├── 自瞄流程图.png ├── picture0.png ├── picture1.png ├── others ├── libMVSDK.so ├── MVCAMSDK_X64.lib ├── libmvsdk.dylib ├── include │ ├── camera │ │ ├── CameraApi.h │ │ ├── CameraImage.h │ │ ├── CameraApiLoad.h │ │ ├── CameraDefine.H │ │ ├── CameraGrabber.h │ │ ├── CameraStatus.h │ │ ├── wrapper_head.h │ │ ├── video_wrapper.h │ │ ├── camera_wrapper.h │ │ └── camera_status.h │ ├── systime.h │ ├── constants.h │ ├── options.h │ ├── serial.h │ ├── additions.h │ ├── config │ │ └── setconfig.h │ └── log.h └── src │ ├── camera │ ├── video_wrapper.cpp │ └── camera_wrapper.cpp │ ├── systime.cpp │ ├── additions.cpp │ └── options.cpp ├── tools ├── monitor.bat ├── para │ ├── conv1_b │ ├── conv2_b │ ├── conv3_b │ ├── fc2_b │ ├── fc1_b │ ├── conv1_w │ └── conv2_w ├── auto-pull.sh ├── monitor.sh ├── create-startup.sh ├── analysis.py └── TrainCNN │ ├── cv_grab.py │ ├── forward.py │ ├── generate.py │ ├── grab.py │ └── backward.py ├── .gitignore ├── armor ├── src │ ├── armor_finder │ │ ├── standby_state │ │ │ └── standby_state.cpp │ │ ├── searching_state │ │ │ └── searching_state.cpp │ │ ├── send_target │ │ │ └── send_target.cpp │ │ ├── tracking_state │ │ │ └── tracking_state.cpp │ │ ├── anti_top │ │ │ └── anti_top.cpp │ │ ├── armor_box │ │ │ └── armor_box.cpp │ │ ├── armor_finder.cpp │ │ └── find │ │ │ ├── find_light_blobs.cpp │ │ │ └── find_armor_box.cpp │ └── show_images │ │ └── show_images.cpp └── include │ ├── show_images │ └── show_images.h │ └── armor_finder │ ├── classifier │ └── classifier.h │ └── armor_finder.h ├── energy ├── include │ └── energy │ │ ├── constant.h │ │ ├── param_struct_define.h │ │ └── energy.h └── src │ └── energy │ ├── judge │ ├── judge_time.cpp │ ├── judge_shoot.cpp │ └── judge_mode.cpp │ ├── change │ ├── mode_change.cpp │ ├── target_change.cpp │ └── multiple_mode.cpp │ ├── get │ ├── get_predict_point.cpp │ ├── get_center.cpp │ ├── get_polar_angle.cpp │ ├── get_aim_point.cpp │ └── get_guess_point.cpp │ ├── clear │ ├── clear.cpp │ └── energy_init.cpp │ ├── run.cpp │ ├── mark │ └── mark.cpp │ ├── energy.cpp │ ├── find │ └── target_finder.cpp │ ├── calibrate │ ├── split.cpp │ └── structing.cpp │ ├── send │ └── send.cpp │ ├── tool │ └── tool.cpp │ └── show │ └── show.cpp ├── LICENSE ├── CMakeLists.txt ├── main.cpp └── README.md /自瞄流程图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/自瞄流程图.png -------------------------------------------------------------------------------- /picture0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/picture0.png -------------------------------------------------------------------------------- /picture1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/picture1.png -------------------------------------------------------------------------------- /others/libMVSDK.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/libMVSDK.so -------------------------------------------------------------------------------- /tools/monitor.bat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/tools/monitor.bat -------------------------------------------------------------------------------- /others/MVCAMSDK_X64.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/MVCAMSDK_X64.lib -------------------------------------------------------------------------------- /others/libmvsdk.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/libmvsdk.dylib -------------------------------------------------------------------------------- /tools/para/conv1_b: -------------------------------------------------------------------------------- 1 | 6 2 | -0.13957113 3 | 0.06878903 4 | 0.031124229 5 | 1.4302016 6 | -0.1433692 7 | 1.1473336 8 | -------------------------------------------------------------------------------- /others/include/camera/CameraApi.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraApi.h -------------------------------------------------------------------------------- /others/include/camera/CameraImage.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraImage.h -------------------------------------------------------------------------------- /others/include/camera/CameraApiLoad.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraApiLoad.h -------------------------------------------------------------------------------- /others/include/camera/CameraDefine.H: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraDefine.H -------------------------------------------------------------------------------- /others/include/camera/CameraGrabber.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraGrabber.h -------------------------------------------------------------------------------- /others/include/camera/CameraStatus.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinyang-go/SJTU-RM-CV-2019/HEAD/others/include/camera/CameraStatus.h -------------------------------------------------------------------------------- /tools/auto-pull.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd $(dirname $0)/../ 4 | timeout 8 git pull 5 | cd cmake-build-debug 6 | cmake .. 7 | make -j4 8 | -------------------------------------------------------------------------------- /tools/para/conv2_b: -------------------------------------------------------------------------------- 1 | 10 2 | -0.6072942 3 | 1.0412933 4 | -0.34075898 5 | 1.2761911 6 | 0.16558713 7 | -0.11275745 8 | -0.20701186 9 | 1.0942192 10 | 0.511572 11 | 0.07464832 12 | -------------------------------------------------------------------------------- /tools/para/conv3_b: -------------------------------------------------------------------------------- 1 | 14 2 | 0.55702305 3 | 1.1578506 4 | 0.4233094 5 | 1.0468913 6 | 0.102127574 7 | -0.11887622 8 | 0.7552765 9 | 0.8270396 10 | -0.037355777 11 | 0.863297 12 | 0.42967162 13 | 0.36658964 14 | 0.751224 15 | 0.5695708 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug 2 | build 3 | .idea 4 | Mark 5 | armor_box_photo 6 | tools/TrainCNN/.idea 7 | tools/TrainCNN/model 8 | tools/TrainCNN/__pycache__ 9 | others/include/config/config.h 10 | others/MV-UB31-Group0.config 11 | .DS_Store 12 | video -------------------------------------------------------------------------------- /tools/para/fc2_b: -------------------------------------------------------------------------------- 1 | 15 2 | -0.68187517 3 | -0.013023868 4 | -0.012667711 5 | 0.12630615 6 | -0.24749278 7 | -0.4987089 8 | 0.3616302 9 | 0.23901181 10 | 0.32909712 11 | -0.25681627 12 | 0.21683879 13 | 0.72209626 14 | -0.2969338 15 | 0.35322607 16 | 0.6401797 17 | -------------------------------------------------------------------------------- /armor/src/armor_finder/standby_state/standby_state.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #include 6 | 7 | bool ArmorFinder::stateStandBy() { 8 | state = SEARCHING_STATE; 9 | return true; 10 | } 11 | 12 | -------------------------------------------------------------------------------- /tools/monitor.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # usage: monitor.sh "" 4 | # And then when ever the exe shutdown it will be automatically restart 5 | 6 | exe=$1 7 | while true; do 8 | state=`ps aux | grep "$1" | grep -v grep | grep -v $0` 9 | if [ ! "$state" ]; then 10 | exec $exe & 11 | echo "restart $exe" 12 | fi 13 | sleep 0.5 14 | done 15 | -------------------------------------------------------------------------------- /others/src/camera/video_wrapper.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | 5 | #include "camera/video_wrapper.h" 6 | 7 | 8 | VideoWrapper::VideoWrapper(const std::string &filename) { 9 | video.open(filename); 10 | } 11 | 12 | VideoWrapper::~VideoWrapper() = default; 13 | 14 | 15 | bool VideoWrapper::init() { 16 | return video.isOpened(); 17 | } 18 | 19 | bool VideoWrapper::read(cv::Mat &src) { 20 | return video.read(src); 21 | } 22 | -------------------------------------------------------------------------------- /energy/include/energy/constant.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #ifndef CONSTANT_H 5 | #define CONSTANT_H 6 | 7 | #include "additions.h" 8 | const int BIG = 1; 9 | const int SMALL = 0; 10 | const int SRC_WIDTH = 320; 11 | const int SRC_HEIGHT = 240; 12 | const int CLOCKWISE = 1; 13 | const int ANTICLOCKWISE = -1; 14 | //const float ATTACK_DISTANCE = 718.0;//cm 15 | const float ATTACK_DISTANCE = 750.0;//cm 16 | const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm 17 | 18 | 19 | #endif //CONSTANT_H 20 | 21 | -------------------------------------------------------------------------------- /tools/create-startup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "#!/bin/bash" > $2/startup-run 4 | echo "echo sjturm | sudo -S cpufreq-set -g performance" >> $2/startup-run 5 | echo "mkdir $1/Mark" >> $2/startup-run 6 | echo "mkdir $1/gimbal_video" >> $2/startup-run 7 | echo "mkdir $1/armor_box_photo" >> $2/startup-run 8 | echo "gnome-terminal -- bash -c \"echo sjturm | sudo -S $1/tools/monitor.sh \\\"$2/run --run-with-camera --save-video --save-mark --show-armor-box --wait-uart --save-labelled-boxes\\\"\"" >> $2/startup-run 9 | chmod +x $2/startup-run 10 | -------------------------------------------------------------------------------- /others/include/systime.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-31. 3 | // 4 | // 提供一个多平台统一的精确到毫秒的系统时间接口 5 | // 该时间仅用于表示相对时间 6 | 7 | #ifndef _PLATFORM_H_ 8 | #define _PLATFORM_H_ 9 | 10 | typedef double systime; 11 | 12 | void getsystime(systime &t); 13 | double getTimeIntervalms(const systime &now, const systime &last); 14 | 15 | #if defined(Linux) || defined(Darwin) 16 | #include 17 | #elif defined(Windows) 18 | #include 19 | #else 20 | #error "nonsupport platform." 21 | #endif 22 | 23 | #endif /* _PLATFORM_H_ */ 24 | -------------------------------------------------------------------------------- /others/include/constants.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-8. 3 | // 4 | 5 | #ifndef _CONSTANTS_H_ 6 | #define _CONSTANTS_H_ 7 | 8 | #define PI (3.14159265459) 9 | 10 | #define ENEMY_BLUE 0 11 | #define ENEMY_RED 1 12 | 13 | #define ALLY_BLUE ENEMY_RED 14 | #define ALLY_RED ENEMY_BLUE 15 | 16 | #define BIG_ENERGY_STATE 'b' 17 | #define SMALL_ENERGY_STATE 's' 18 | #define ARMOR_STATE 'a' 19 | 20 | #define FOCUS_PIXAL_8MM (1488) 21 | #define FOCUS_PIXAL_5MM (917) 22 | #define FOCUS_PIXAL FOCUS_PIXAL_5MM 23 | 24 | #endif /* _CONSTANTS_H */ 25 | -------------------------------------------------------------------------------- /others/include/camera/wrapper_head.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zhikun on 18-11-18. 3 | // 4 | 5 | #ifndef STEREOVISION_FROM_VIDEO_FILE_WRAPPER_HEAD_H 6 | #define STEREOVISION_FROM_VIDEO_FILE_WRAPPER_HEAD_H 7 | 8 | #include 9 | 10 | /** 11 | * @brief A virtual class for wrapper of camera and video files 12 | */ 13 | class WrapperHead { 14 | public: 15 | virtual ~WrapperHead() = default;; 16 | virtual bool init() = 0; 17 | virtual bool read(cv::Mat &src) = 0; 18 | }; 19 | 20 | 21 | 22 | #endif //STEREOVISION_FROM_VIDEO_FILE_WRAPPER_HEAD_H 23 | -------------------------------------------------------------------------------- /energy/src/energy/judge/judge_time.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-11. 3 | // 4 | 5 | 6 | #include "energy/energy.h" 7 | #include "log.h" 8 | 9 | using namespace std; 10 | using namespace cv; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于判断guess模式是否超时 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | bool Energy::isGuessingTimeout() { 17 | systime cur_time; 18 | getsystime(cur_time); 19 | return getTimeIntervalms(cur_time, time_start_guess) > 1000; 20 | }; -------------------------------------------------------------------------------- /others/include/options.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #ifndef _OPTIONS_H_ 6 | #define _OPTIONS_H_ 7 | 8 | #ifdef PATH 9 | #define PROJECT_DIR PATH 10 | #else 11 | #define PROJECT_DIR "" 12 | #endif 13 | 14 | extern bool show_armor_box; 15 | extern bool show_armor_boxes; 16 | extern bool show_light_blobs; 17 | extern bool show_origin; 18 | extern bool run_with_camera; 19 | extern bool save_video; 20 | extern bool wait_uart; 21 | extern bool save_labelled_boxes; 22 | extern bool show_process; 23 | extern bool show_energy; 24 | extern bool save_mark; 25 | extern bool show_info; 26 | extern bool run_by_frame; 27 | 28 | 29 | void processOptions(int argc, char **argv); 30 | 31 | #endif /* _OPTIONS_H_ */ 32 | -------------------------------------------------------------------------------- /energy/src/energy/change/mode_change.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-10. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | 11 | //---------------------------------------------------------------------------------------------------------------------- 12 | // 此函数用于判断是否应当继续保持猜测模式 13 | // --------------------------------------------------------------------------------------------------------------------- 14 | bool Energy::stayGuessing(){ 15 | if (change_target || isGuessingTimeout()) { 16 | is_predicting = true; 17 | is_guessing = false; 18 | LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!")); 19 | return false; 20 | } 21 | return true; 22 | } -------------------------------------------------------------------------------- /armor/include/show_images/show_images.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #ifndef _SHOW_IMAGES_H_ 6 | #define _SHOW_IMAGES_H_ 7 | 8 | #include 9 | #include 10 | 11 | // 12 | void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes); 13 | void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box); 14 | void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs); 15 | void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes); 16 | void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos); 17 | 18 | #endif /* _SHOW_IMAGES_H_ */ 19 | -------------------------------------------------------------------------------- /energy/src/energy/change/target_change.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-10. 3 | // 4 | #include "energy/energy.h" 5 | 6 | using namespace std; 7 | using namespace cv; 8 | 9 | 10 | //---------------------------------------------------------------------------------------------------------------------- 11 | // 此函数用于判断目标是否切换 12 | // --------------------------------------------------------------------------------------------------------------------- 13 | void Energy::changeTarget() { 14 | if (abs(last_target_polar_angle_judge_change - target_polar_angle) < 20 || 15 | abs(last_target_polar_angle_judge_change - target_polar_angle) > 340) { 16 | change_target = false; 17 | } else { 18 | change_target = true; 19 | } 20 | last_target_polar_angle_judge_change = target_polar_angle; 21 | } -------------------------------------------------------------------------------- /others/src/systime.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-31. 3 | // 4 | #include 5 | 6 | #if defined(Linux) || defined(Darwin) 7 | 8 | static systime getsystime(){ 9 | timeval tv; 10 | gettimeofday(&tv, nullptr); 11 | return tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0; 12 | } 13 | 14 | void getsystime(systime &t) { 15 | static systime time_base = getsystime(); 16 | timeval tv; 17 | gettimeofday(&tv, nullptr); 18 | t = tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0 - time_base; 19 | } 20 | 21 | #elif defined(Windows) 22 | 23 | void getsystime(systime &t){ 24 | SYSTEMTIME tv; 25 | GetLocalTime(&tv); 26 | t = tv.wMilliseconds + tv.wSecond * 1000.0; 27 | } 28 | 29 | #else 30 | #error "nonsupport platform." 31 | #endif 32 | 33 | 34 | double getTimeIntervalms(const systime &now, const systime &last) { 35 | return now - last; 36 | } 37 | -------------------------------------------------------------------------------- /energy/src/energy/get/get_predict_point.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | 5 | #include "energy/energy.h" 6 | #include "energy/constant.h" 7 | 8 | using namespace cv; 9 | using std::cout; 10 | using std::endl; 11 | using std::vector; 12 | 13 | 14 | //---------------------------------------------------------------------------------------------------------------------- 15 | // 此函数获取预测点坐标 16 | // --------------------------------------------------------------------------------------------------------------------- 17 | void Energy::getPredictPoint(cv::Point target_point) { 18 | if (is_big) { 19 | if (energy_rotation_direction == 1) predict_rad = predict_rad_norm; 20 | else if (energy_rotation_direction == -1) predict_rad = -predict_rad_norm; 21 | rotate(target_point); 22 | } else if (is_small) predict_point = target_point; 23 | } 24 | 25 | -------------------------------------------------------------------------------- /energy/src/energy/judge/judge_shoot.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-11. 3 | // 4 | 5 | #include "energy/energy.h" 6 | #include "log.h" 7 | #include "config/setconfig.h" 8 | 9 | using namespace std; 10 | using namespace cv; 11 | 12 | 13 | 14 | //---------------------------------------------------------------------------------------------------------------------- 15 | // 此函数用于判断云台坐标系下是否可以发弹 16 | // --------------------------------------------------------------------------------------------------------------------- 17 | void Energy::judgeShoot() { 18 | if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) { 19 | shoot = 2; 20 | // is_predicting = false; 21 | // is_guessing = true; 22 | // start_guess = true; 23 | // getsystime(time_start_guess); 24 | // LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); 25 | } else 26 | shoot = 1; 27 | } 28 | -------------------------------------------------------------------------------- /tools/para/fc1_b: -------------------------------------------------------------------------------- 1 | 60 2 | -0.193765 3 | -0.23972291 4 | 0.07138973 5 | 0.32630488 6 | -0.12837179 7 | 0.15837212 8 | -0.171342 9 | 0.15245913 10 | 0.2655143 11 | 0.5132081 12 | -0.36182725 13 | 0.5721853 14 | 1.1356937 15 | 0.478751 16 | -0.19061308 17 | -0.4349855 18 | -0.037231974 19 | 0.82266986 20 | 0.021297721 21 | 0.27819484 22 | -0.3700498 23 | -0.080103956 24 | 0.118309975 25 | -0.120765805 26 | -0.21436372 27 | 0.38247055 28 | 0.9884826 29 | -0.4042391 30 | -0.21405952 31 | -1.1451919 32 | -0.07339987 33 | 0.63600576 34 | 0.39354753 35 | -0.14708714 36 | 0.5753427 37 | -0.06053154 38 | 0.6068468 39 | -0.24616803 40 | -0.18896966 41 | -0.06904445 42 | -0.137959 43 | -0.75066173 44 | -0.20227952 45 | 0.17195949 46 | -0.13245444 47 | -0.10060697 48 | 0.40073013 49 | -0.9563751 50 | 0.031663556 51 | 0.38514042 52 | -0.061627824 53 | 0.14056061 54 | 0.7515243 55 | 0.7903934 56 | 0.8401122 57 | 0.29314804 58 | 0.2987165 59 | 0.39764544 60 | 0.3022464 61 | -0.29335007 62 | -------------------------------------------------------------------------------- /energy/src/energy/change/multiple_mode.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-23. 3 | // 4 | 5 | #include "energy/energy.h" 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | 11 | //---------------------------------------------------------------------------------------------------------------------- 12 | // 此函数用于切换预测模式和猜测模式，但最终未使用 13 | // --------------------------------------------------------------------------------------------------------------------- 14 | void Energy::multipleMode(cv::Mat &src) { 15 | if (is_predicting) { 16 | getPredictPoint(target_point); 17 | getAimPoint(predict_point); 18 | judgeShoot(); 19 | sendEnergy(); 20 | } else if (is_guessing && stayGuessing()) { 21 | findFans(src); 22 | if (show_energy)showFans("fans", src); 23 | if (save_mark)writeDownMark(src); 24 | guessTarget(); 25 | if (show_energy)showGuessTarget("guess", src); 26 | getPredictPoint(guess_point); 27 | getAimPoint(predict_point); 28 | sendEnergy(); 29 | } 30 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 xinyang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /others/include/camera/video_wrapper.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zhikun on 18-11-16. 3 | // wrapper for video read from file 4 | // 5 | 6 | #ifndef STEREOVISION_FROM_VIDEO_FILE_VIDEO_WRAPPER_H 7 | #define STEREOVISION_FROM_VIDEO_FILE_VIDEO_WRAPPER_H 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "wrapper_head.h" 15 | 16 | 17 | class VideoWrapper:public WrapperHead { 18 | public: 19 | VideoWrapper(const std::string& filename); 20 | ~VideoWrapper(); 21 | 22 | 23 | /** 24 | * @brief initialize cameras 25 | * @return bool value: whether it success 26 | */ 27 | bool init() final; 28 | 29 | 30 | /** 31 | * @brief read images from camera 32 | * @param src_left : output source video of left camera 33 | * @param src_right : output source video of right camera 34 | * @return bool value: whether the reading is successful 35 | */ 36 | bool read(cv::Mat &src) final; 37 | private: 38 | cv::VideoCapture video; 39 | 40 | }; 41 | 42 | 43 | #endif //STEREOVISION_FROM_VIDEO_FILE_VIDEO_WRAPPER_H 44 | -------------------------------------------------------------------------------- /armor/src/armor_finder/searching_state/searching_state.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { 11 | if (findArmorBox(src, target_box)) { // 在原图中寻找目标，并返回是否找到 12 | if (last_box.rect != cv::Rect2d() && 13 | (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) && 14 | anti_switch_cnt++ < 3) { // 判断当前目标和上次有效目标是否为同一个目标 15 | target_box = ArmorBox(); // 并给３帧的时间，试图找到相同目标 16 | LOGM("anti-switch!"); // 即刚发生目标切换内的３帧内不发送目标位置 17 | return false; // 可以一定程度避免频繁多目标切换 18 | } else { 19 | anti_switch_cnt = 0; 20 | return true; 21 | } 22 | } else { 23 | target_box = ArmorBox(); 24 | anti_switch_cnt++; 25 | return false; 26 | } 27 | } 28 | 29 | /* 30 | bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { 31 | if (findArmorBox(src, target_box)) { 32 | return true; 33 | } else { 34 | target_box = ArmorBox(); 35 | return false; 36 | } 37 | } 38 | */ -------------------------------------------------------------------------------- /energy/src/energy/get/get_center.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-10. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | #include "config/setconfig.h" 7 | 8 | 9 | using namespace std; 10 | using namespace cv; 11 | 12 | 13 | 14 | //---------------------------------------------------------------------------------------------------------------------- 15 | // 此函数用于对心 16 | // --------------------------------------------------------------------------------------------------------------------- 17 | void Energy::getCenter(){ 18 | int compensate_yaw = 0, compensate_pitch = 0; 19 | if (mcu_data.enemy_color == ENEMY_BLUE) { 20 | compensate_yaw = RED_COMPENSATE_YAW; 21 | compensate_pitch = RED_COMPENSATE_PITCH; 22 | } else if (mcu_data.enemy_color == ENEMY_RED) { 23 | compensate_yaw = BLUE_COMPENSATE_YAW; 24 | compensate_pitch = BLUE_COMPENSATE_PITCH; 25 | } 26 | double dx = -(circle_center_point.x - 320 - compensate_yaw); 27 | double dy = -(circle_center_point.y - 240 - compensate_pitch); 28 | yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; 29 | pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; 30 | if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) { 31 | shoot = 2; 32 | } else 33 | shoot = 1; 34 | } 35 | -------------------------------------------------------------------------------- /energy/src/energy/judge/judge_mode.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-12. 3 | // 4 | 5 | #include "energy/energy.h" 6 | #include "log.h" 7 | 8 | using namespace std; 9 | using namespace cv; 10 | 11 | 12 | //---------------------------------------------------------------------------------------------------------------------- 13 | // 此函数用于判断大小符 14 | // --------------------------------------------------------------------------------------------------------------------- 15 | void Energy::judgeMode() { 16 | getRecentTargetArmorCenters(); 17 | if (recent_target_armor_centers.size() < 30) { 18 | return; 19 | } else { 20 | if (abs(recent_target_armor_centers.back() - recent_target_armor_centers.front()) > 10) { 21 | if(last_mode!=BIG){ 22 | is_big = true; 23 | is_small = false; 24 | energy_mode_init = false; 25 | LOGM(STR_CTR(WORD_CYAN,"start big!")); 26 | } 27 | last_mode = BIG; 28 | } else { 29 | if(last_mode!=SMALL){ 30 | is_big = false; 31 | is_small = true; 32 | energy_mode_init = false; 33 | LOGM(STR_CTR(WORD_GREEN,"start small!")); 34 | } 35 | last_mode = SMALL; 36 | } 37 | } 38 | } -------------------------------------------------------------------------------- /energy/src/energy/clear/clear.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-11. 3 | // 4 | 5 | #include "energy/energy.h" 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | extern McuData mcu_data; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于清空各vector 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::clearAll() { 17 | fans.clear(); 18 | armors.clear(); 19 | flow_strip_fans.clear(); 20 | target_armors.clear(); 21 | flow_strips.clear(); 22 | } 23 | 24 | 25 | //---------------------------------------------------------------------------------------------------------------------- 26 | // 此函数用于图像预处理 27 | // --------------------------------------------------------------------------------------------------------------------- 28 | void Energy::initImage(cv::Mat &src) { 29 | if (src.type() == CV_8UC3){ 30 | cvtColor(src, src, COLOR_BGR2GRAY); 31 | } 32 | if (mcu_data.enemy_color == ENEMY_BLUE){ 33 | threshold(src, src, energy_part_param_.RED_GRAY_THRESH, 255, THRESH_BINARY); 34 | } else if(mcu_data.enemy_color == ENEMY_RED){ 35 | threshold(src, src, energy_part_param_.BLUE_GRAY_THRESH, 255, THRESH_BINARY); 36 | } 37 | if (show_process) imshow("bin", src); 38 | if (show_energy || show_process)waitKey(1); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /others/include/camera/camera_wrapper.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zhikun on 18-11-7. 3 | // 4 | 5 | #ifndef _CAMERA_WRAPPER_H_ 6 | #define _CAMERA_WRAPPER_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #ifdef Windows 13 | #include "camera/CameraApi.h" 14 | #elif defined(Linux) || defined(Darwin) 15 | #include 16 | #endif 17 | 18 | class CameraWrapper: public WrapperHead { 19 | friend void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext); 20 | private: 21 | const std::string name; 22 | int mode; 23 | 24 | bool init_done; 25 | 26 | unsigned char* rgb_buffer; 27 | int camera_cnts; 28 | int camera_status; 29 | tSdkCameraDevInfo camera_enum_list[2]; 30 | int h_camera; 31 | char camera_name[32]; 32 | 33 | tSdkCameraCapbility tCapability; 34 | tSdkFrameHead frame_info; 35 | BYTE *pby_buffer; 36 | IplImage* iplImage; 37 | int channel; 38 | 39 | RoundQueue src_queue; 40 | public: 41 | int gain; 42 | int exposure; 43 | 44 | CameraWrapper(int exposure, int gain, int camera_mode=1, const std::string &n="NULL"); 45 | ~CameraWrapper() final; 46 | 47 | bool init() final; 48 | bool read(cv::Mat& src) final; 49 | bool readRaw(cv::Mat& src); 50 | bool readProcessed(cv::Mat& src); 51 | bool readCallback(cv::Mat& src); 52 | }; 53 | 54 | 55 | #endif /* _CAMERA_WRAPPER_H_ */ 56 | -------------------------------------------------------------------------------- /energy/src/energy/get/get_polar_angle.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | 6 | using namespace cv; 7 | using namespace std; 8 | 9 | 10 | //---------------------------------------------------------------------------------------------------------------------- 11 | // 此函数获取目标装甲板极坐标角度 12 | // --------------------------------------------------------------------------------------------------------------------- 13 | void Energy::getTargetPolarAngle() { 14 | target_polar_angle = static_cast(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), 15 | (target_point.x - circle_center_point.x))); 16 | } 17 | 18 | 19 | 20 | //---------------------------------------------------------------------------------------------------------------------- 21 | // 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符 22 | // --------------------------------------------------------------------------------------------------------------------- 23 | void Energy::getRecentTargetArmorCenters() { 24 | if (change_target) { 25 | while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();//目标切换时清空 26 | return; 27 | } 28 | if (recent_target_armor_centers.size() < 30)recent_target_armor_centers.push(target_polar_angle); 29 | else { 30 | recent_target_armor_centers.pop(); 31 | recent_target_armor_centers.push(target_polar_angle); 32 | } 33 | } -------------------------------------------------------------------------------- /energy/src/energy/run.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 3/5/19. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | #include "config/setconfig.h" 7 | #include "options.h" 8 | 9 | using namespace std; 10 | using namespace cv; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数为能量机关模式主控制流函数，且步兵仅拥有云台摄像头 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::run(cv::Mat &src) { 17 | clearAll(); 18 | initImage(src); 19 | 20 | if (show_process)imshow("bin", src); 21 | if (findArmors(src) < 1)return; 22 | if (show_energy)showArmors("armor", src); 23 | if (!findFlowStripFan(src)) { 24 | if (!findFlowStripWeak(src)) return; 25 | } else { 26 | if (show_energy)showFlowStripFan("strip fan", src); 27 | if (!findTargetInFlowStripFan()) return; 28 | if (!findFlowStrip(src)) return; 29 | } 30 | findCenterROI(src); 31 | if (show_energy)showFlowStrip("strip", src); 32 | if (!findCenterR(src)) return; 33 | if (show_energy)showCenterR("R", src); 34 | fans_cnt = findFans(src); 35 | if (show_energy)showFans("fans", src); 36 | 37 | changeTarget(); 38 | getTargetPolarAngle(); 39 | 40 | if (is_big && energy_rotation_init) { 41 | initRotation(); 42 | return; 43 | } 44 | getPredictPoint(target_point); 45 | getAimPoint(predict_point); 46 | judgeShoot(); 47 | sendEnergy(); 48 | if (save_mark)writeDownMark(src); 49 | } 50 | 51 | 52 | -------------------------------------------------------------------------------- /others/include/serial.h: -------------------------------------------------------------------------------- 1 | #ifndef _SERIAL_H_ 2 | #define _SERIAL_H_ 3 | 4 | #ifdef Windows 5 | 6 | #include 7 | 8 | class Serial 9 | { 10 | public: 11 | Serial(UINT baud = CBR_115200, char parity = 'N', UINT databits = 8, UINT stopsbits = 1, DWORD dwCommEvents = EV_RXCHAR); 12 | ~Serial(); 13 | 14 | bool InitPort(UINT portNo = 1, UINT baud = CBR_9600, char parity = 'N', UINT databits = 8, UINT stopsbits = 1, DWORD dwCommEvents = EV_RXCHAR); 15 | UINT GetBytesInCOM() const ; 16 | bool WriteData(const unsigned char* pData, unsigned int length); 17 | bool ReadData(unsigned char* buffer, unsigned int length); 18 | private: 19 | bool openPort(UINT portNo); 20 | void ClosePort(); 21 | void ErrorHandler(); 22 | private: 23 | HANDLE hComm; 24 | UINT portNo; 25 | UINT baud; 26 | char parity; 27 | UINT databits; 28 | UINT stopsbits; 29 | DWORD dwCommEvents; 30 | }; 31 | 32 | #elif defined(Linux) || defined(Darwin) 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | class Serial { 40 | private: 41 | int fd; 42 | int nSpeed; 43 | char nEvent; 44 | int nBits; 45 | int nStop; 46 | 47 | int set_opt(int fd, int nSpeed, char nEvent, int nBits, int nStop); 48 | 49 | public: 50 | Serial(int nSpeed = 115200, char nEvent = 'N', int nBits = 8, int nStop = 1); 51 | ~Serial(); 52 | 53 | bool InitPort(int nSpeed = 115200, char nEvent = 'N', int nBits = 8, int nStop = 1); 54 | // int GetBytesInCOM() const ; 55 | bool WriteData(const unsigned char* pData, unsigned int length); 56 | bool ReadData(unsigned char* buffer, unsigned int length); 57 | }; 58 | 59 | #endif 60 | 61 | #endif /* _SERIAL_H_ */ 62 | -------------------------------------------------------------------------------- /tools/analysis.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | fsend = open("send.info") 5 | frecv = open("recv.info") 6 | 7 | send_lines = fsend.readlines()[:-1] 8 | recv_lines = frecv.readlines()[:-1] 9 | 10 | send_infos = [line.split(" ") for line in send_lines] 11 | recv_infos = [line.split(" ") for line in recv_lines] 12 | 13 | send_times = [float(info[0]) for info in send_infos] 14 | recv_times = [float(info[0]) for info in recv_infos] 15 | 16 | send_yaw = [float(info[1]) for info in send_infos] 17 | TargetAngle= [float(info[1]) for info in recv_infos] 18 | RealAngle = [float(info[2]) for info in recv_infos] 19 | 20 | length = min(len(send_times), len(recv_times)) 21 | 22 | send_times = send_times[:length] 23 | recv_times = recv_times[:length] 24 | send_yaw = send_yaw[:length] 25 | TargetAngle= TargetAngle[:length] 26 | RealAngle = RealAngle[:length] 27 | 28 | time_base = min(send_times[0], recv_times[0]) 29 | 30 | send_times = np.array(send_times) - time_base 31 | recv_times = np.array(recv_times) - time_base 32 | 33 | TargetAngle = TargetAngle[:300] 34 | RealAngle = RealAngle[:300] 35 | send_times = send_times[:300] 36 | recv_times = recv_times[:300] 37 | send_yaw = send_yaw[:300] 38 | 39 | TargetAngle = np.array(TargetAngle) 40 | TargetAngle -= TargetAngle.mean() 41 | RealAngle = np.array(RealAngle) 42 | RealAngle -= RealAngle.mean() 43 | 44 | plt.plot(send_times, send_yaw, label="send-yaw") 45 | plt.plot(recv_times, TargetAngle,label="TargetAngle") 46 | plt.plot(recv_times, RealAngle, label="RealAngle") 47 | plt.scatter(send_times, send_yaw, label="send-yaw") 48 | plt.scatter(recv_times, TargetAngle,label="TargetAngle") 49 | plt.scatter(recv_times, RealAngle, label="RealAngle") 50 | plt.legend() 51 | plt.show() 52 | 53 | -------------------------------------------------------------------------------- /energy/src/energy/mark/mark.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-5. 3 | // 4 | 5 | #include "energy/energy.h" 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | extern McuData mcu_data; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于记录操作手的微调dx和dy 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::writeDownMark(cv::Mat &src) { 17 | if (fans_cnt >= 2) { 18 | FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); 19 | if (fp_delta) { 20 | fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, 21 | mcu_data.delta_y + manual_delta_y); 22 | fclose(fp_delta); 23 | } 24 | } 25 | FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a"); 26 | if (fp_data) { 27 | if(mcu_data.mark == 1){ 28 | fprintf(fp_data, "PID: %s\t", "new"); 29 | } else { 30 | fprintf(fp_data, "PID: %s\t", "default"); 31 | } 32 | if(is_big){ 33 | fprintf(fp_data, "state: %s\t", "big"); 34 | } else if(is_small){ 35 | fprintf(fp_data, "state: %s\t", "small"); 36 | } 37 | fprintf(fp_data, "fps: %d\t", curr_fps); 38 | fprintf(fp_data, "fans_cnt: %d\t", fans_cnt); 39 | fprintf(fp_data, "shoot: %d\t", int(shoot)); 40 | fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation); 41 | fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, 42 | mcu_data.delta_y + manual_delta_y); 43 | fclose(fp_data); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /energy/src/energy/energy.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | 7 | using namespace cv; 8 | using std::cout; 9 | using std::endl; 10 | using std::vector; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数为能量机关构造函数，只要程序不重启就不会重新构造 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color), 17 | src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1), 18 | src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1) { 19 | initEnergy(); 20 | initEnergyPartParam(); 21 | 22 | is_big = false; 23 | is_small = false; 24 | } 25 | 26 | 27 | //---------------------------------------------------------------------------------------------------------------------- 28 | // 此函数为能量机关析构函数，设置为默认 29 | // --------------------------------------------------------------------------------------------------------------------- 30 | Energy::~Energy() = default; 31 | 32 | 33 | //---------------------------------------------------------------------------------------------------------------------- 34 | // 此函数为大能量机关再初始化函数 35 | // --------------------------------------------------------------------------------------------------------------------- 36 | void Energy::setEnergyInit() { 37 | initEnergy(); 38 | initEnergyPartParam(); 39 | 40 | FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r"); 41 | if (fp) { 42 | fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y); 43 | fclose(fp); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /energy/src/energy/find/target_finder.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | 5 | #include "energy/energy.h" 6 | #include "log.h" 7 | 8 | using namespace cv; 9 | using std::cout; 10 | using std::endl; 11 | using std::vector; 12 | 13 | 14 | 15 | 16 | //---------------------------------------------------------------------------------------------------------------------- 17 | // 此函数在流动条区域内寻找装甲板 18 | // --------------------------------------------------------------------------------------------------------------------- 19 | bool Energy::findTargetInFlowStripFan() { 20 | // Mat draw(480,640,CV_8UC3,Scalar(0,0,0)); 21 | for (auto &candidate_flow_strip_fan: flow_strip_fans) { 22 | // Point2f vertices[4]; //定义矩形的4个顶点 23 | // candidate_flow_strip_fan.points(vertices); //计算矩形的4个顶点 24 | // for (int i = 0; i < 4; i++) 25 | // line(draw, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); 26 | int i = 0; 27 | for (i = 0; i < armors.size(); ++i) { 28 | std::vector intersection; 29 | if (rotatedRectangleIntersection(armors.at(i), candidate_flow_strip_fan, intersection) == 0) 30 | continue;//返回0表示没有重合面积 31 | double cur_contour_area = contourArea(intersection); 32 | // cout< energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) { 34 | target_armors.emplace_back(armors.at(i)); 35 | } 36 | } 37 | } 38 | // imshow("draw",draw); 39 | // waitKey(); 40 | // cout << "target armor cnt: " << target_armors.size() << endl; 41 | if (target_armors.empty()) { 42 | if (show_info)cout << "find target armor false" << endl; 43 | return false; 44 | } else { 45 | return true; 46 | } 47 | 48 | 49 | } -------------------------------------------------------------------------------- /others/include/additions.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sjturm on 19-5-17. 3 | // 4 | 5 | #ifndef _ADDITIONS_H_ 6 | #define _ADDITIONS_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | // 单片机端回传数据结构体 13 | struct McuData { 14 | float curr_yaw; // 当前云台yaw角度 15 | float curr_pitch; // 当前云台pitch角 16 | uint8_t state; // 当前状态，自瞄-大符-小符 17 | uint8_t mark; // 云台角度标记位 18 | uint8_t anti_top; // 是否为反陀螺模式 19 | uint8_t enemy_color; // 敌方颜色 20 | int delta_x; // 能量机关x轴补偿量 21 | int delta_y; // 能量机关y轴补偿量 22 | }; 23 | 24 | extern McuData mcu_data; 25 | // 串口接收函数 26 | void uartReceive(Serial *pSerial); 27 | // 相机断线重连函数 28 | bool checkReconnect(bool is_camera_connect); 29 | // 视频保存函数 30 | void saveVideos(const cv::Mat &src); 31 | //　原始图像显示函数 32 | void showOrigin(const cv::Mat &src); 33 | // 图像裁剪函数 34 | void extract(cv::Mat &src); 35 | 36 | double getPointLength(const cv::Point2f &p); 37 | 38 | // 循环队列 39 | template 40 | class RoundQueue { 41 | private: 42 | type data[length]; 43 | int head; 44 | int tail; 45 | public: 46 | RoundQueue() : head(0), tail(0) {}; 47 | 48 | constexpr int size() const { 49 | return length; 50 | }; 51 | 52 | bool empty() const { 53 | return head == tail; 54 | }; 55 | 56 | void push(const type &obj) { 57 | data[head] = obj; 58 | head = (head + 1) % length; 59 | if (head == tail) { 60 | tail = (tail + 1) % length; 61 | } 62 | }; 63 | 64 | bool pop(type &obj) { 65 | if (empty()) return false; 66 | obj = data[tail]; 67 | tail = (tail + 1) % length; 68 | return true; 69 | }; 70 | 71 | type &operator[](int idx) { 72 | while (tail + idx < 0) idx += length; 73 | return data[(tail + idx) % length]; 74 | }; 75 | }; 76 | 77 | #endif /* _ADDITIONS_H_ */ 78 | -------------------------------------------------------------------------------- /others/include/config/setconfig.h: -------------------------------------------------------------------------------- 1 | // 本文件定义了在不同步兵车辆上运行时需要根据车辆情况而发生变化的参数 2 | // 以下所有参数都给定了默认值，如果想要修改，推荐在该文件目录下新建文件config.h，并在config.h中写定自定义参数 3 | 4 | #ifndef _SETCONFIG_H_ 5 | #define _SETCONFIG_H_ 6 | 7 | 8 | #ifdef WITH_CONFIG 9 | #include 10 | #else 11 | #warning "Without config.h" 12 | #endif 13 | 14 | #ifndef ARMOR_CAMERA_EXPOSURE 15 | #define ARMOR_CAMERA_EXPOSURE (10) 16 | #endif 17 | 18 | #ifndef ENERGY_CAMERA_EXPOSURE 19 | #define ENERGY_CAMERA_EXPOSURE (10) 20 | #endif 21 | 22 | #ifndef CAMERA_RED_GAIN 23 | #define CAMERA_RED_GAIN (100) 24 | #endif 25 | 26 | #ifndef CAMERA_GREEN_GAIN 27 | #define CAMERA_GREEN_GAIN (100) 28 | #endif 29 | 30 | #ifndef CAMERA_BLUE_GAIN 31 | #define CAMERA_BLUE_GAIN (100) 32 | #endif 33 | 34 | 35 | #ifndef ARMOR_CAMERA_GAIN 36 | #define ARMOR_CAMERA_GAIN (64) 37 | #endif 38 | #ifndef ENERGY_CAMERA_GAIN 39 | #define ENERGY_CAMERA_GAIN (30) 40 | #endif 41 | 42 | #ifndef YAW_AIM_KD 43 | #define YAW_AIM_KD (0.4) 44 | #endif 45 | #ifndef YAW_AIM_KP 46 | #define YAW_AIM_KP (0.75) 47 | #endif 48 | #ifndef YAW_AIM_KI 49 | #define YAW_AIM_KI (0.01) 50 | #endif 51 | #ifndef PITCH_AIM_KD 52 | #define PITCH_AIM_KD (0.4) 53 | #endif 54 | #ifndef PITCH_AIM_KP 55 | #define PITCH_AIM_KP (0.75) 56 | #endif 57 | #ifndef PITCH_AIM_KI 58 | #define PITCH_AIM_KI (0.01) 59 | #endif 60 | 61 | #ifndef RED_COMPENSATE_YAW 62 | #define RED_COMPENSATE_YAW (5) 63 | #endif 64 | #ifndef RED_COMPENSATE_PITCH 65 | #define RED_COMPENSATE_PITCH (74) 66 | #endif 67 | 68 | #ifndef BLUE_COMPENSATE_YAW 69 | #define BLUE_COMPENSATE_YAW (5) 70 | #endif 71 | #ifndef BLUE_COMPENSATE_PITCH 72 | #define BLUE_COMPENSATE_PITCH (74) 73 | #endif 74 | 75 | #ifndef EXTRA_DELTA_X 76 | #define EXTRA_DELTA_X (0) 77 | #endif 78 | #ifndef EXTRA_DELTA_Y 79 | #define EXTRA_DELTA_Y (10) 80 | #endif 81 | 82 | //#define GIMBAL_FLIP_MODE (-1) 83 | //#define CHASSIS_FLIP_MODE (-1) 84 | //#define WITH_TIME_BASED_CAMERA_GAIN 85 | #define WITH_COUNT_FPS 86 | 87 | #endif /* SETCONFIG_H */ 88 | -------------------------------------------------------------------------------- /armor/src/armor_finder/send_target/send_target.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-11. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) { 10 | static short x_tmp, y_tmp, z_tmp; 11 | uint8_t buff[10]; 12 | 13 | #ifdef WITH_COUNT_FPS 14 | static time_t last_time = time(nullptr); 15 | static int fps; 16 | time_t t = time(nullptr); 17 | if (last_time != t) { 18 | last_time = t; 19 | cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; 20 | fps = 0; 21 | } 22 | fps += 1; 23 | #endif 24 | 25 | x_tmp = static_cast(x * (32768 - 1) / 100); 26 | y_tmp = static_cast(y * (32768 - 1) / 100); 27 | z_tmp = static_cast(z * (32768 - 1) / 1000); 28 | 29 | buff[0] = 's'; 30 | buff[1] = static_cast((x_tmp >> 8) & 0xFF); 31 | buff[2] = static_cast((x_tmp >> 0) & 0xFF); 32 | buff[3] = static_cast((y_tmp >> 8) & 0xFF); 33 | buff[4] = static_cast((y_tmp >> 0) & 0xFF); 34 | buff[5] = static_cast((z_tmp >> 8) & 0xFF); 35 | buff[6] = static_cast((z_tmp >> 0) & 0xFF); 36 | buff[7] = static_cast((shoot_delay >> 8) & 0xFF); 37 | buff[8] = static_cast((shoot_delay >> 0) & 0xFF); 38 | buff[9] = 'e'; 39 | // if(buff[7]<<8 | buff[8]) 40 | // cout << (buff[7]<<8 | buff[8]) << endl; 41 | return serial.WriteData(buff, sizeof(buff)); 42 | } 43 | 44 | bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { 45 | if (target_box.rect == cv::Rect2d()) return false; 46 | if (shoot_delay) { 47 | LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay); 48 | } 49 | auto rect = target_box.rect; 50 | double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X; 51 | double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y; 52 | double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; 53 | double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; 54 | double dist = DISTANCE_HEIGHT / rect.height; 55 | return sendTarget(serial, yaw, -pitch, dist, shoot_delay); 56 | } 57 | -------------------------------------------------------------------------------- /armor/include/armor_finder/classifier/classifier.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-4-19. 3 | // 4 | // 为了一时方便，使用循环和Eigen自行编写的CNN前向传播类。 5 | // 没有显著的性能损失。 6 | // 但类定义了网络结构，同时实现的操作较少，可扩展性较差 7 | 8 | #ifndef _CLASSIFIER_H_ 9 | #define _CLASSIFIER_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | using namespace Eigen; 19 | 20 | class Classifier { 21 | private: 22 | bool state; // 标志分类器是否正确初始化 23 | 24 | // 所有网络参数 25 | vector> conv1_w, conv2_w, conv3_w; 26 | vector conv1_b, conv2_b, conv3_b; 27 | MatrixXd fc1_w, fc2_w; 28 | VectorXd fc1_b, fc2_b; 29 | // 读取网络参数的函数 30 | vector> load_conv_w(const string &file); 31 | vector load_conv_b(const string &file); 32 | MatrixXd load_fc_w(const string &file); 33 | VectorXd load_fc_b(const string &file); 34 | // 目前支持的所有操作 35 | MatrixXd softmax(const MatrixXd &input); 36 | MatrixXd relu(const MatrixXd &input); 37 | MatrixXd leaky_relu(const MatrixXd &input, float alpha); 38 | vector> apply_bias(const vector> &input, const vector &bias); 39 | vector> relu(const vector> &input); 40 | vector> leaky_relu(const vector> &input, float alpha); 41 | vector> max_pool(const vector> &input, int size); 42 | vector> mean_pool(const vector> &input, int size); 43 | vector> pand(const vector> &input, int val); 44 | MatrixXd conv(const MatrixXd &filter, const MatrixXd &input); 45 | vector> conv2(const vector> &filter, const vector> &input); 46 | MatrixXd flatten(const vector> &input); 47 | 48 | public: 49 | explicit Classifier(const string &folder); 50 | ~Classifier() = default; 51 | 52 | MatrixXd calculate(const vector> &input); 53 | explicit operator bool() const; 54 | int operator()(const cv::Mat &image); 55 | }; 56 | 57 | 58 | #endif /* _CLASSIFIER_H */ 59 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 2 | 3 | PROJECT(SJTU-RM-CV) 4 | SET(CMAKE_CXX_STANDARD 11) 5 | SET(CMAKE_BUILD_TYPE RELEASE) 6 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") 7 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") 9 | SET(BIN_NAME "run") 10 | 11 | 12 | FIND_FILE(CONFIG_FOUND "config.h" "others/include/config") 13 | if (CONFIG_FOUND) 14 | MESSAGE("Found config.h") 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CONFIG") 16 | endif() 17 | 18 | 19 | FIND_PROGRAM(CCACHE_FOUND ccache) 20 | IF(CCACHE_FOUND) 21 | SET_PROPERTY(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 22 | SET_PROPERTY(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 23 | MESSAGE("< Use ccache for compiler >") 24 | ENDIF() 25 | 26 | FIND_PACKAGE(Eigen3 REQUIRED) 27 | FIND_PACKAGE(OpenCV 3 REQUIRED) 28 | FIND_PACKAGE(Threads) 29 | 30 | LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others) 31 | 32 | INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 33 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/energy/include) 34 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include) 35 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include) 36 | 37 | FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp") 38 | ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles}) 39 | 40 | TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT}) 41 | TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS}) 42 | IF (CMAKE_SYSTEM_NAME MATCHES "Linux") 43 | MESSAGE(STATUS "current platform: Linux ") 44 | TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so") 45 | ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Windows") 46 | MESSAGE(STATUS "current platform: Windows") 47 | TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib") 48 | ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Darwin") 49 | MESSAGE(STATUS "current platform: Mac") 50 | TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libmvsdk.dylib") 51 | ELSE () 52 | MESSAGE(STATUS "Unsupport platform: ${CMAKE_SYSTEM_NAME}") 53 | ENDIF() 54 | 55 | ADD_CUSTOM_TARGET(create-startup COMMAND "${PROJECT_SOURCE_DIR}/tools/create-startup.sh" "${PROJECT_SOURCE_DIR}" "${CMAKE_BINARY_DIR}") 56 | ADD_CUSTOM_TARGET(train-cnn COMMAND "gnome-terminal" "--" "bash" "-c" "${PROJECT_SOURCE_DIR}/tools/TrainCNN/backward.py") -------------------------------------------------------------------------------- /energy/src/energy/get/get_aim_point.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-6. 3 | // 4 | 5 | #include "energy/energy.h" 6 | #include "energy/constant.h" 7 | #include "config/setconfig.h" 8 | 9 | using namespace std; 10 | using namespace cv; 11 | 12 | extern McuData mcu_data; 13 | 14 | //---------------------------------------------------------------------------------------------------------------------- 15 | // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 16 | // --------------------------------------------------------------------------------------------------------------------- 17 | void Energy::getAimPoint(cv::Point target_point_) { 18 | float target_polar_angle_ = static_cast(180 / PI * atan2(-1 * (target_point_.y - circle_center_point.y), 19 | (target_point_.x - circle_center_point.x))); 20 | 21 | if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) { 22 | extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90; 23 | extra_delta_y = -EXTRA_DELTA_Y * target_polar_angle_ / 90; 24 | } else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) { 25 | extra_delta_x = -EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; 26 | extra_delta_y = -EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; 27 | } else { 28 | extra_delta_x = 0; 29 | extra_delta_y = 0; 30 | } 31 | 32 | int compensate_yaw = 0, compensate_pitch = 0; 33 | if (mcu_data.enemy_color == ENEMY_BLUE) { 34 | compensate_yaw = RED_COMPENSATE_YAW; 35 | compensate_pitch = RED_COMPENSATE_PITCH; 36 | } else if (mcu_data.enemy_color == ENEMY_RED) { 37 | compensate_yaw = BLUE_COMPENSATE_YAW; 38 | compensate_pitch = BLUE_COMPENSATE_PITCH; 39 | } 40 | 41 | double dx = -(target_point_.x - 320 - compensate_yaw - mcu_data.delta_x - manual_delta_x - extra_delta_x); 42 | double dy = -(target_point_.y - 240 - compensate_pitch - mcu_data.delta_y - manual_delta_y - extra_delta_y); 43 | yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; 44 | pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; 45 | // cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; 46 | // cout << "mcu_data.delta_x: " << mcu_data.delta_x << '\t' << "mcu_data.delta_y: " << mcu_data.delta_y << endl; 47 | // cout << "manual delta: " << manual_delta_x << '\t' << manual_delta_y << endl; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /armor/src/armor_finder/tracking_state/tracking_state.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | #define LOG_LEVEL LOG_NONE 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | bool ArmorFinder::stateTrackingTarget(cv::Mat &src) { 11 | auto pos = target_box.rect; 12 | if(!tracker->update(src, pos)){ // 使用KCFTracker进行追踪 13 | target_box = ArmorBox(); 14 | LOGW("Track fail!"); 15 | return false; 16 | } 17 | if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){ 18 | target_box = ArmorBox(); 19 | LOGW("Track out range!"); 20 | return false; 21 | } 22 | 23 | // 获取相较于追踪区域两倍长款的区域，用于重新搜索，获取灯条信息 24 | cv::Rect2d bigger_rect; 25 | bigger_rect.x = pos.x - pos.width / 2.0; 26 | bigger_rect.y = pos.y - pos.height / 2.0; 27 | bigger_rect.height = pos.height * 2; 28 | bigger_rect.width = pos.width * 2; 29 | bigger_rect &= cv::Rect2d(0, 0, 640, 480); 30 | cv::Mat roi = src(bigger_rect).clone(); 31 | 32 | ArmorBox box; 33 | // 在区域内重新搜索。 34 | if(findArmorBox(roi, box)) { // 如果成功获取目标，则利用搜索区域重新更新追踪器 35 | target_box = box; 36 | target_box.rect.x += bigger_rect.x; //　添加roi偏移量 37 | target_box.rect.y += bigger_rect.y; 38 | for(auto &blob : target_box.light_blobs){ 39 | blob.rect.center.x += bigger_rect.x; 40 | blob.rect.center.y += bigger_rect.y; 41 | } 42 | tracker = TrackerToUse::create(); 43 | tracker->init(src, target_box.rect); 44 | }else{ // 如果没有成功搜索目标，则使用判断是否跟丢。 45 | roi = src(pos).clone(); 46 | if(classifier){ // 分类器可用，使用分类器判断。 47 | cv::resize(roi, roi, cv::Size(48, 36)); 48 | if(classifier(roi) == 0){ 49 | target_box = ArmorBox(); 50 | LOGW("Track classify fail range!"); 51 | return false; 52 | } 53 | }else{ //　分类器不可用，使用常规方法判断 54 | cv::Mat roi_gray; 55 | cv::cvtColor(roi, roi_gray, CV_RGB2GRAY); 56 | cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY); 57 | contour_area = cv::countNonZero(roi_gray); 58 | if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){ 59 | target_box = ArmorBox(); 60 | return false; 61 | } 62 | } 63 | target_box.rect = pos; 64 | target_box.light_blobs.clear(); 65 | } 66 | return true; 67 | } 68 | -------------------------------------------------------------------------------- /armor/src/armor_finder/anti_top/anti_top.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-15. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | template 10 | static double mean(RoundQueue &vec) { 11 | double sum = 0; 12 | for (int i = 0; i < vec.size(); i++) { 13 | sum += vec[i]; 14 | } 15 | return sum / length; 16 | } 17 | 18 | static systime getFrontTime(const vector time_seq, const vector angle_seq) { 19 | double A = 0, B = 0, C = 0, D = 0; 20 | int len = time_seq.size(); 21 | for (int i = 0; i < len; i++) { 22 | A += angle_seq[i] * angle_seq[i]; 23 | B += angle_seq[i]; 24 | C += angle_seq[i] * time_seq[i]; 25 | D += time_seq[i]; 26 | cout << "(" << angle_seq[i] << ", " << time_seq[i] << ") "; 27 | } 28 | double b = (A * D - B * C) / (len * A - B * B); 29 | cout << b << endl; 30 | return b; 31 | } 32 | 33 | void ArmorFinder::antiTop() { 34 | if (target_box.rect == cv::Rect2d()) return; 35 | // 判断是否发生装甲目标切换。 36 | // 记录切换前一段时间目标装甲的角度和时间 37 | // 通过线性拟合计算出角度为0时对应的时间点 38 | // 通过两次装甲角度为零的时间差计算陀螺旋转周期 39 | // 根据旋转周期计算下一次装甲出现在角度为零的时间点 40 | if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) { 41 | auto front_time = getFrontTime(time_seq, angle_seq); 42 | auto once_periodms = getTimeIntervalms(front_time, last_front_time); 43 | // if (abs(once_periodms - top_periodms[-1]) > 50) { 44 | // sendBoxPosition(0); 45 | // return; 46 | // } 47 | LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms); 48 | top_periodms.push(once_periodms); 49 | auto periodms = mean(top_periodms); 50 | systime curr_time; 51 | getsystime(curr_time); 52 | uint16_t shoot_delay = front_time + periodms * 2 - curr_time; 53 | if (anti_top_cnt < 4) { 54 | sendBoxPosition(0); 55 | } else if (abs(once_periodms - top_periodms[-1]) > 50) { 56 | sendBoxPosition(0); 57 | } else { 58 | sendBoxPosition(shoot_delay); 59 | } 60 | time_seq.clear(); 61 | angle_seq.clear(); 62 | last_front_time = front_time; 63 | } else { 64 | time_seq.emplace_back(frame_time); 65 | double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X; 66 | double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; 67 | angle_seq.emplace_back(yaw); 68 | sendBoxPosition(0); 69 | } 70 | anti_top_cnt++; 71 | } 72 | 73 | -------------------------------------------------------------------------------- /energy/src/energy/get/get_guess_point.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sun on 19-7-11. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | 11 | //---------------------------------------------------------------------------------------------------------------------- 12 | // 此函数获得猜测的击打点 13 | // --------------------------------------------------------------------------------------------------------------------- 14 | bool Energy::guessTarget() { 15 | vector all_fan_angles; 16 | float angle; 17 | for (const auto &fan : fans) { 18 | angle = static_cast(180 / PI * atan2(-1 * (fan.center.y - circle_center_point.y), 19 | (fan.center.x - circle_center_point.x))); 20 | if (angle < 0)angle += 360; 21 | all_fan_angles.emplace_back(angle); 22 | } 23 | angle = target_polar_angle; 24 | if (angle < 0)angle += 360; 25 | all_fan_angles.emplace_back(angle); 26 | if(all_fan_angles.size()==5){ 27 | LOGM(STR_CTR(WORD_PURPLE,"all lighted!")); 28 | return false; 29 | } 30 | 31 | sort(all_fan_angles.begin(), all_fan_angles.end()); 32 | float min_angle = all_fan_angles.at(0); 33 | float max_angle = all_fan_angles.at(all_fan_angles.size() - 1); 34 | float base_angle = min_angle; 35 | while (base_angle > 72)base_angle -= 72; 36 | if (start_guess) { 37 | int i = 0; 38 | for (i = 1; i < all_fan_angles.size(); ++i) { 39 | if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) { 40 | guess_polar_angle = min_angle + 72 * i; 41 | break; 42 | } 43 | } 44 | if (i == all_fan_angles.size()) guess_polar_angle = (max_angle + 72); 45 | start_guess = false; 46 | guess_devide = devide(guess_polar_angle); 47 | } else if (abs(base_angle - last_base_angle) > 20) { 48 | if (energy_rotation_direction == CLOCKWISE)guess_devide = (guess_devide + 4) % 5; 49 | else guess_devide = (guess_devide + 1) % 5; 50 | guess_polar_angle = base_angle + guess_devide * 72; 51 | } else { 52 | guess_polar_angle = base_angle + guess_devide * 72; 53 | } 54 | if (guess_polar_angle > 180)guess_polar_angle -= 360; 55 | double radius = pointDistance(target_point, circle_center_point); 56 | guess_point.x = circle_center_point.x + radius * cos(PI / 180.0 * guess_polar_angle); 57 | guess_point.y = circle_center_point.y - radius * sin(PI / 180.0 * guess_polar_angle); 58 | last_base_angle = base_angle; 59 | return true; 60 | } 61 | -------------------------------------------------------------------------------- /energy/src/energy/calibrate/split.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | 6 | using namespace cv; 7 | using std::cout; 8 | using std::endl; 9 | using std::vector; 10 | 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于分离拜耳阵列 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red) { 17 | uchar* data; 18 | uchar* bayer_data[2]; 19 | for (int i = 0; i < src.rows; ++i) { 20 | data = src.ptr(i); 21 | bayer_data[0] = blue.ptr(i / 2); 22 | for (int j = 0; j < blue.cols; ++j, data += 2) { 23 | bayer_data[0][j] = *data; 24 | } 25 | data = src.ptr(++i) + 1; 26 | bayer_data[1] = red.ptr(i / 2); 27 | for (int j = 0; j < red.cols; ++j, data += 2) { 28 | bayer_data[1][j] = *data; 29 | } 30 | } 31 | } 32 | 33 | 34 | 35 | //---------------------------------------------------------------------------------------------------------------------- 36 | // 此函数对图像进行通道分离处理 37 | // --------------------------------------------------------------------------------------------------------------------- 38 | void Energy::imagePreprocess(cv::Mat &src) { 39 | if(src.type() == CV_8UC1) 40 | { 41 | splitBayerBG(src, src_blue, src_red); 42 | if(ally_color == ALLY_RED) 43 | { 44 | // src = src_red - src_blue; 45 | src = src_red; 46 | }else if(ally_color == ALLY_BLUE){ 47 | // src = src_blue - src_red; 48 | src = src_blue; 49 | } 50 | } 51 | 52 | 53 | else if(src.type() == CV_8UC3) 54 | { 55 | std::vector channels; 56 | split(src, channels); 57 | resize(channels.at(0), src_blue, Size(src.size().width/2, src.size().height/2)); 58 | resize(channels.at(1), src_green, Size(src.size().width/2, src.size().height/2)); 59 | resize(channels.at(2), src_red, Size(src.size().width/2, src.size().height/2)); 60 | if(ally_color == ALLY_RED) 61 | { 62 | // src = src_red-src_blue; 63 | src=src_red; 64 | }else if(ally_color == ALLY_BLUE){ 65 | // src = src_blue-src_red; 66 | src=src_blue; 67 | } 68 | } 69 | cv::resize(src, src, cv::Size(src.size().width * 2, src.size().height * 2), 2); 70 | threshold(src, src, energy_part_param_.SPLIT_GRAY_THRESH, 255, THRESH_BINARY); 71 | 72 | } 73 | -------------------------------------------------------------------------------- /armor/src/armor_finder/armor_box/armor_box.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-13. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) : 9 | rect(pos), light_blobs(blobs), box_color(color), id(i) {}; 10 | 11 | // 获取装甲板中心点 12 | cv::Point2f ArmorBox::getCenter() const { 13 | return cv::Point2f( 14 | rect.x + rect.width / 2, 15 | rect.y + rect.height / 2 16 | ); 17 | } 18 | 19 | // 获取两个灯条中心点的间距 20 | double ArmorBox::getBlobsDistance() const { 21 | if (light_blobs.size() == 2) { 22 | auto &x = light_blobs[0].rect.center; 23 | auto &y = light_blobs[1].rect.center; 24 | return sqrt((x.x - y.x) * (x.x - y.x) + (x.y - y.y) * (x.y - y.y)); 25 | } else { 26 | return 0; 27 | } 28 | } 29 | 30 | // 获取灯条长度和间距的比例 31 | double ArmorBox::lengthDistanceRatio() const { 32 | if (light_blobs.size() == 2) { 33 | return max(light_blobs[0].length, light_blobs[1].length) 34 | / getBlobsDistance(); 35 | } else { 36 | return 100; 37 | } 38 | } 39 | 40 | // 计算装甲板到摄像头的距离 41 | double ArmorBox::getBoxDistance() const { 42 | if (light_blobs.size() == 2) { 43 | return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length); 44 | } else { 45 | return DISTANCE_HEIGHT / rect.height; 46 | } 47 | } 48 | 49 | // 判断装甲板的正对还是侧对(已弃用，误差过大) 50 | ArmorBox::BoxOrientation ArmorBox::getOrientation() const { 51 | if (light_blobs.size() != 2) { 52 | return UNKNOWN; 53 | } 54 | switch (id) { 55 | case R1: 56 | case R7: 57 | case R8: 58 | case B1: 59 | case B7: 60 | case B8: 61 | if (lengthDistanceRatio() < 0.26) { 62 | return FRONT; 63 | } else { 64 | return SIDE; 65 | } 66 | case R2: 67 | case R3: 68 | case R4: 69 | case R5: 70 | case B2: 71 | case B3: 72 | case B4: 73 | case B5: 74 | if (lengthDistanceRatio() < 0.51) { 75 | return FRONT; 76 | } else { 77 | return SIDE; 78 | } 79 | default: 80 | return UNKNOWN; 81 | } 82 | } 83 | // 装甲板的优先级比较 84 | bool ArmorBox::operator<(const ArmorBox &box) const { 85 | if (id != box.id) { 86 | if (box_color == BOX_BLUE) { 87 | return prior_blue[id2name[id]] < prior_blue[id2name[box.id]]; 88 | } else { 89 | return prior_red[id2name[id]] < prior_red[id2name[box.id]]; 90 | } 91 | } else { 92 | auto d1 = (rect.x - IMAGE_CENTER_X) * (rect.x - IMAGE_CENTER_X) 93 | + (rect.y - IMAGE_CENTER_Y) * (rect.y - IMAGE_CENTER_Y); 94 | auto d2 = (box.rect.x - IMAGE_CENTER_X) * (box.rect.x - IMAGE_CENTER_X) 95 | + (box.rect.y - IMAGE_CENTER_Y) * (box.rect.y - IMAGE_CENTER_Y); 96 | return d1 < d2; 97 | } 98 | } 99 | 100 | -------------------------------------------------------------------------------- /tools/TrainCNN/cv_grab.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | import cv2 3 | import numpy as np 4 | import mvsdk 5 | 6 | def main_loop(): 7 | # 枚举相机 8 | DevList = mvsdk.CameraEnumerateDevice() 9 | nDev = len(DevList) 10 | if nDev < 1: 11 | print("No camera was found!") 12 | return 13 | 14 | for i, DevInfo in enumerate(DevList): 15 | print("{}: {} {}".format(i, DevInfo.GetFriendlyName(), DevInfo.GetPortType())) 16 | i = 0 if nDev == 1 else int(input("Select camera: ")) 17 | DevInfo = DevList[i] 18 | print(DevInfo) 19 | 20 | # 打开相机 21 | hCamera = 0 22 | try: 23 | hCamera = mvsdk.CameraInit(DevInfo, -1, -1) 24 | except mvsdk.CameraException as e: 25 | print("CameraInit Failed({}): {}".format(e.error_code, e.message) ) 26 | return 27 | 28 | # 获取相机特性描述 29 | cap = mvsdk.CameraGetCapability(hCamera) 30 | 31 | # 判断是黑白相机还是彩色相机 32 | monoCamera = (cap.sIspCapacity.bMonoSensor != 0) 33 | 34 | # 黑白相机让ISP直接输出MONO数据，而不是扩展成R=G=B的24位灰度 35 | if monoCamera: 36 | mvsdk.CameraSetIspOutFormat(hCamera, mvsdk.CAMERA_MEDIA_TYPE_MONO8) 37 | else: 38 | mvsdk.CameraSetIspOutFormat(hCamera, mvsdk.CAMERA_MEDIA_TYPE_BGR8) 39 | 40 | # 相机模式切换成连续采集 41 | mvsdk.CameraSetTriggerMode(hCamera, 0) 42 | 43 | # 手动曝光，曝光时间30ms 44 | mvsdk.CameraSetAeState(hCamera, 0) 45 | mvsdk.CameraSetExposureTime(hCamera, 30 * 1000) 46 | 47 | # 让SDK内部取图线程开始工作 48 | mvsdk.CameraPlay(hCamera) 49 | 50 | # 计算RGB buffer所需的大小，这里直接按照相机的最大分辨率来分配 51 | FrameBufferSize = cap.sResolutionRange.iWidthMax * cap.sResolutionRange.iHeightMax * (1 if monoCamera else 3) 52 | 53 | # 分配RGB buffer，用来存放ISP输出的图像 54 | # 备注：从相机传输到PC端的是RAW数据，在PC端通过软件ISP转为RGB数据（如果是黑白相机就不需要转换格式，但是ISP还有其它处理，所以也需要分配这个buffer） 55 | pFrameBuffer = mvsdk.CameraAlignMalloc(FrameBufferSize, 16) 56 | 57 | while (cv2.waitKey(1) & 0xFF) != ord('q'): 58 | # 从相机取一帧图片 59 | try: 60 | pRawData, FrameHead = mvsdk.CameraGetImageBuffer(hCamera, 200) 61 | mvsdk.CameraImageProcess(hCamera, pRawData, pFrameBuffer, FrameHead) 62 | mvsdk.CameraReleaseImageBuffer(hCamera, pRawData) 63 | 64 | # 此时图片已经存储在pFrameBuffer中，对于彩色相机pFrameBuffer=RGB数据，黑白相机pFrameBuffer=8位灰度数据 65 | # 把pFrameBuffer转换成opencv的图像格式以进行后续算法处理 66 | frame_data = (mvsdk.c_ubyte * FrameHead.uBytes).from_address(pFrameBuffer) 67 | frame = np.frombuffer(frame_data, dtype=np.uint8) 68 | frame = frame.reshape((FrameHead.iHeight, FrameHead.iWidth, 1 if FrameHead.uiMediaType == mvsdk.CAMERA_MEDIA_TYPE_MONO8 else 3) ) 69 | 70 | frame = cv2.resize(frame, (640,480), interpolation = cv2.INTER_LINEAR) 71 | cv2.imshow("Press q to end", frame) 72 | roi = cv2.selectROI("roi", frame) 73 | roi = frame[roi[1]:roi[1]+roi[3], roi[0]:roi[0]+roi[2]] 74 | print(roi) 75 | cv2.imshow("box", roi) 76 | 77 | 78 | except mvsdk.CameraException as e: 79 | if e.error_code != mvsdk.CAMERA_STATUS_TIME_OUT: 80 | print("CameraGetImageBuffer failed({}): {}".format(e.error_code, e.message) ) 81 | 82 | # 关闭相机 83 | mvsdk.CameraUnInit(hCamera) 84 | 85 | # 释放帧缓存 86 | mvsdk.CameraAlignFree(pFrameBuffer) 87 | 88 | def main(): 89 | try: 90 | main_loop() 91 | finally: 92 | cv2.destroyAllWindows() 93 | 94 | main() 95 | -------------------------------------------------------------------------------- /energy/src/energy/send/send.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | #include 6 | #include "log.h" 7 | #include "config/setconfig.h" 8 | 9 | using namespace std; 10 | 11 | #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于发送能量机关数据 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::sendEnergy() { 17 | sum_yaw += yaw_rotation; 18 | sum_pitch += pitch_rotation; 19 | float yaw_I_component = YAW_AIM_KI * sum_yaw; 20 | float pitch_I_component = PITCH_AIM_KI * sum_pitch; 21 | MINMAX(yaw_I_component, -2, 2); 22 | MINMAX(pitch_I_component, -2, 2); 23 | 24 | double tmp_yaw = yaw_rotation; 25 | double tmp_pitch = pitch_rotation; 26 | yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw + 27 | YAW_AIM_KD * (yaw_rotation - last_yaw); 28 | pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch + 29 | PITCH_AIM_KD * (pitch_rotation - last_pitch); 30 | 31 | last_yaw = tmp_yaw; 32 | last_pitch = tmp_pitch; 33 | 34 | if (change_target) { 35 | sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换 36 | } else if (is_guessing) { 37 | sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式 38 | } else { 39 | sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹 40 | } 41 | 42 | } 43 | 44 | 45 | //---------------------------------------------------------------------------------------------------------------------- 46 | // 此函数用于发送数据给主控板 47 | // --------------------------------------------------------------------------------------------------------------------- 48 | void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) { 49 | short x_tmp, y_tmp, z_tmp; 50 | uint8_t buff[10]; 51 | 52 | #ifdef WITH_COUNT_FPS 53 | static auto last_time = time(nullptr); 54 | static int fps = 0; 55 | time_t t = time(nullptr); 56 | if (last_time != t) { 57 | last_time = t; 58 | cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl; 59 | curr_fps = fps; 60 | fps = 0; 61 | } 62 | fps += 1; 63 | #endif 64 | 65 | x_tmp = static_cast(x * (32768 - 1) / 100); 66 | y_tmp = static_cast(y * (32768 - 1) / 100); 67 | z_tmp = static_cast(z * (32768 - 1) / 100); 68 | buff[0] = 's'; 69 | buff[1] = static_cast((x_tmp >> 8) & 0xFF); 70 | buff[2] = static_cast((x_tmp >> 0) & 0xFF); 71 | buff[3] = static_cast((y_tmp >> 8) & 0xFF); 72 | buff[4] = static_cast((y_tmp >> 0) & 0xFF); 73 | buff[5] = static_cast((z_tmp >> 8) & 0xFF); 74 | buff[6] = static_cast((z_tmp >> 0) & 0xFF); 75 | buff[7] = static_cast((u >> 8) & 0xFF); 76 | buff[8] = static_cast((u >> 0) & 0xFF); 77 | buff[9] = 'e'; 78 | serial.WriteData(buff, sizeof(buff)); 79 | send_cnt += 1; 80 | // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); 81 | } 82 | -------------------------------------------------------------------------------- /tools/TrainCNN/forward.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | 3 | 4 | def get_weight(shape, regularizer=None): 5 | w = tf.Variable(tf.truncated_normal(shape, stddev=0.1)) 6 | if regularizer is not None: 7 | tf.add_to_collection('losses', tf.contrib.layers.l2_regularizer(regularizer)(w)) 8 | return w 9 | 10 | 11 | def get_bias(shape): 12 | b = tf.Variable(tf.zeros(shape)) 13 | return b 14 | 15 | 16 | def conv2d(x, w): 17 | return tf.nn.conv2d(x, w, strides=[1, 1, 1, 1], padding="VALID") 18 | 19 | 20 | def avg_pool_2x2(x): 21 | return tf.nn.avg_pool(x, ksize=[1, 2, 2, 1], strides=[1, 2, 2, 1], padding="VALID") 22 | 23 | 24 | def max_pool_2x2(x): 25 | return tf.nn.max_pool(x, ksize=[1, 2, 2, 1], strides=[1, 2, 2, 1], padding="VALID") 26 | 27 | 28 | # 第一层卷积核大小 29 | CONV1_KERNAL_SIZE = 5 30 | 31 | # 第一层卷积输出通道数 32 | CONV1_OUTPUT_CHANNELS = 6 33 | 34 | # 第二层卷积核大小 35 | CONV2_KERNAL_SIZE = 3 36 | 37 | # 第二层卷积输出通道数 38 | CONV2_OUTPUT_CHANNELS = 10 39 | 40 | # 第三层卷积核大小 41 | CONV3_KERNAL_SIZE = 3 42 | 43 | # 第三层卷积输出通道数 44 | CONV3_OUTPUT_CHANNELS = 14 45 | 46 | # 第一层全连接宽度 47 | FC1_OUTPUT_NODES = 60 48 | 49 | # 第二层全连接宽度（输出标签类型数） 50 | FC2_OUTPUT_NODES = 15 51 | 52 | # 输出标签类型数 53 | OUTPUT_NODES = FC2_OUTPUT_NODES 54 | 55 | 56 | def forward(x, regularizer=None, keep_rate=tf.constant(1.0)): 57 | vars = [] 58 | vars_name = [] 59 | nodes = [] 60 | 61 | conv1_w = get_weight( 62 | [CONV1_KERNAL_SIZE, CONV1_KERNAL_SIZE, int(x.shape[3]), CONV1_OUTPUT_CHANNELS] 63 | ) 64 | conv1_b = get_bias([CONV1_OUTPUT_CHANNELS]) 65 | conv1 = tf.nn.relu(tf.nn.bias_add(conv2d(x, conv1_w), conv1_b)) 66 | pool1 = avg_pool_2x2(conv1) 67 | print("conv1: ", conv1.shape) 68 | print("pool1: ", pool1.shape) 69 | vars.extend([conv1_w, conv1_b]) 70 | vars_name.extend(["conv1_w", "conv1_b"]) 71 | nodes.extend([conv1, pool1]) 72 | 73 | conv2_w = get_weight( 74 | [CONV2_KERNAL_SIZE, CONV2_KERNAL_SIZE, CONV1_OUTPUT_CHANNELS, CONV2_OUTPUT_CHANNELS] 75 | ) 76 | conv2_b = get_bias([CONV2_OUTPUT_CHANNELS]) 77 | conv2 = tf.nn.relu(tf.nn.bias_add(conv2d(pool1, conv2_w), conv2_b)) 78 | pool2 = avg_pool_2x2(conv2) 79 | print("conv2: ", conv2.shape) 80 | vars.extend([conv2_w, conv2_b]) 81 | vars_name.extend(["conv2_w", "conv2_b"]) 82 | nodes.extend([conv2, pool2]) 83 | 84 | conv3_w = get_weight( 85 | [CONV3_KERNAL_SIZE, CONV3_KERNAL_SIZE, CONV2_OUTPUT_CHANNELS, CONV3_OUTPUT_CHANNELS] 86 | ) 87 | conv3_b = get_bias([CONV3_OUTPUT_CHANNELS]) 88 | conv3 = tf.nn.relu(tf.nn.bias_add(conv2d(pool2, conv3_w), conv3_b)) 89 | print("conv3: ", conv3.shape) 90 | vars.extend([conv3_w, conv3_b]) 91 | vars_name.extend(["conv3_w", "conv3_b"]) 92 | nodes.extend([conv3]) 93 | 94 | conv_shape = conv3.get_shape().as_list() 95 | node = conv_shape[1] * conv_shape[2] * conv_shape[3] 96 | reshaped = tf.reshape(conv3, [-1, node]) 97 | reshaped = tf.nn.dropout(reshaped, keep_rate) 98 | print("reshaped: ", reshaped.shape) 99 | 100 | fc1_w = get_weight([node, FC1_OUTPUT_NODES], regularizer) 101 | fc1_b = get_bias([FC1_OUTPUT_NODES]) 102 | fc1 = tf.nn.relu(tf.matmul(reshaped, fc1_w) + fc1_b) 103 | vars.extend([fc1_w, fc1_b]) 104 | vars_name.extend(["fc1_w", "fc1_b"]) 105 | nodes.extend([fc1]) 106 | 107 | fc2_w = get_weight([FC1_OUTPUT_NODES, FC2_OUTPUT_NODES], regularizer) 108 | fc2_b = get_bias([FC2_OUTPUT_NODES]) 109 | fc2 = tf.matmul(fc1, fc2_w) + fc2_b 110 | vars.extend([fc2_w, fc2_b]) 111 | vars_name.extend(["fc2_w", "fc2_b"]) 112 | nodes.extend([fc2]) 113 | 114 | return nodes, vars, vars_name 115 | -------------------------------------------------------------------------------- /energy/include/energy/param_struct_define.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #ifndef PARAM_STRUCT_DEFINE_H 5 | #define PARAM_STRUCT_DEFINE_H 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using std::vector; 12 | 13 | 14 | 15 | //---------------------------------------------------------------------------------------------------------------------- 16 | // 此结构体包括能量机关参数 17 | // --------------------------------------------------------------------------------------------------------------------- 18 | struct EnergyPartParam { 19 | int RED_GRAY_THRESH;//红方二值化阈值 20 | int BLUE_GRAY_THRESH;//蓝方二值化阈值 21 | int SPLIT_GRAY_THRESH;//通道分离二值化阈值 22 | 23 | long FAN_CONTOUR_AREA_MAX;//扇叶面积最大值 24 | long FAN_CONTOUR_AREA_MIN;//扇叶面积最小值 25 | long FAN_CONTOUR_LENGTH_MIN;//扇叶长边长度最小值 26 | long FAN_CONTOUR_LENGTH_MAX;//扇叶长边长度最大值 27 | long FAN_CONTOUR_WIDTH_MIN;//扇叶宽边长度最小值 28 | long FAN_CONTOUR_WIDTH_MAX;//扇叶宽边长度最大值 29 | float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值 30 | float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值 31 | float FAN_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 32 | 33 | long ARMOR_CONTOUR_AREA_MAX;//装甲板面积最大值 34 | long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值 35 | long ARMOR_CONTOUR_LENGTH_MIN;//装甲板长边长度最小值 36 | long ARMOR_CONTOUR_WIDTH_MIN;//装甲板长边长度最大值 37 | long ARMOR_CONTOUR_LENGTH_MAX;//装甲板宽边长度最小值 38 | long ARMOR_CONTOUR_WIDTH_MAX;//装甲板宽边长度最大值 39 | float ARMOR_CONTOUR_HW_RATIO_MAX;//装甲板长宽比最大值 40 | float ARMOR_CONTOUR_HW_RATIO_MIN;//装甲板长宽比最小值 41 | float ARMOR_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 42 | 43 | long CENTER_R_CONTOUR_AREA_MAX;//风车中心R面积最大值 44 | long CENTER_R_CONTOUR_AREA_MIN;//风车中心R面积最小值 45 | long CENTER_R_CONTOUR_LENGTH_MIN;//风车中心R长边长度最小值 46 | long CENTER_R_CONTOUR_WIDTH_MIN;//风车中心R长边长度最大值 47 | long CENTER_R_CONTOUR_LENGTH_MAX;//风车中心R宽边长度最小值 48 | long CENTER_R_CONTOUR_WIDTH_MAX;//风车中心R宽边长度最大值 49 | float CENTER_R_CONTOUR_HW_RATIO_MAX;//风车中心R长宽比最大值 50 | float CENTER_R_CONTOUR_HW_RATIO_MIN;//风车中心R长宽比最小值 51 | float CENTER_R_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 52 | float CENTER_R_CONTOUR_INTERSETION_AREA_MIN;//中心R占ROI区的面积最小值 53 | 54 | long FLOW_STRIP_FAN_CONTOUR_AREA_MAX;//流动条扇叶（待击打）面积最大值 55 | long FLOW_STRIP_FAN_CONTOUR_AREA_MIN;//流动条扇叶（待击打）面积最小值 56 | long FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN;//流动条扇叶（待击打）长边长度最小值 57 | long FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN;//流动条扇叶（待击打）宽边长度最小值 58 | long FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX;//流动条扇叶（待击打）长边长度最大值 59 | long FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX;//流动条扇叶（待击打）宽边长度最大值 60 | float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX;//流动条扇叶（待击打）长宽比最大值 61 | float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN;//流动条扇叶（待击打）长宽比最小值 62 | float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//流动条扇叶轮廓占旋转矩形面积比最小值 63 | float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN;//流动条扇叶占旋转矩形面积比最小值 64 | 65 | long FLOW_STRIP_CONTOUR_AREA_MAX;//流动条（待击打）面积最大值 66 | long FLOW_STRIP_CONTOUR_AREA_MIN;//流动条（待击打）面积最小值 67 | long FLOW_STRIP_CONTOUR_LENGTH_MIN;//流动条（待击打）长边长度最小值 68 | long FLOW_STRIP_CONTOUR_WIDTH_MIN;//流动条（待击打）宽边长度最小值 69 | long FLOW_STRIP_CONTOUR_LENGTH_MAX;//流动条（待击打）长边长度最大值 70 | long FLOW_STRIP_CONTOUR_WIDTH_MAX;//流动条（待击打）宽边长度最大值 71 | float FLOW_STRIP_CONTOUR_HW_RATIO_MAX;//流动条（待击打）长宽比最大值 72 | float FLOW_STRIP_CONTOUR_HW_RATIO_MIN;//流动条（待击打）长宽比最小值 73 | float FLOW_STRIP_CONTOUR_AREA_RATIO_MIN;//流动条占旋转矩形面积比最小值 74 | float FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN;//流动条占旋转矩形面积比最小值 75 | 76 | float TWIN_ANGEL_MAX;//两个理论上相等的角度在计算时具有的可能最大差值 77 | long TARGET_INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积 78 | 79 | long TWIN_POINT_MAX;//两个点相同时距离最大值 80 | 81 | long STRIP_ARMOR_DISTANCE_MIN;//流动条中心和目标装甲板中心距离最小值 82 | long STRIP_ARMOR_DISTANCE_MAX;//流动条中心和目标装甲板中心距离最大值 83 | }; 84 | 85 | 86 | #endif //PARAM_STRUCT_DEFINE_H 87 | 88 | -------------------------------------------------------------------------------- /others/src/additions.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sjturm on 19-5-17. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define RECEIVE_LOG_LEVEL LOG_MSG 19 | 20 | using namespace std; 21 | using namespace cv; 22 | 23 | extern WrapperHead *video; 24 | 25 | extern Serial serial; 26 | extern uint8_t last_state; 27 | 28 | extern ArmorFinder armor_finder; 29 | extern Energy energy; 30 | 31 | void uartReceive(Serial *pSerial) { 32 | char buffer[40]; 33 | LOGM(STR_CTR(WORD_LIGHT_WHITE, "data receive start!")); 34 | while (true) { 35 | memset(buffer, 0, sizeof(buffer)); 36 | pSerial->ReadData((uint8_t *) buffer, sizeof(mcu_data)+1); 37 | if (buffer[sizeof(mcu_data)] == '\n') { 38 | memcpy(&mcu_data, buffer, sizeof(mcu_data)); 39 | // LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark); 40 | // LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch); 41 | // LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y); 42 | // static int t = time(nullptr); 43 | // static int cnt = 0; 44 | // if(time(nullptr) > t){ 45 | // t = time(nullptr); 46 | // LOGM("fps:%d", cnt); 47 | // cnt = 0; 48 | // }else{ 49 | // cnt++; 50 | // } 51 | }else{ 52 | // LOGW("corrupt data!"); 53 | } 54 | } 55 | } 56 | 57 | cv::VideoWriter initVideoWriter(const std::string &filename_prefix) { 58 | cv::VideoWriter video; 59 | std::ifstream in(filename_prefix + "cnt.txt"); 60 | int cnt = 0; 61 | if (in.is_open()) { 62 | in >> cnt; 63 | in.close(); 64 | } 65 | std::string file_name = filename_prefix + std::to_string(cnt) + ".avi"; 66 | cnt++; 67 | std::ofstream out(filename_prefix + "cnt.txt"); 68 | if (out.is_open()) { 69 | out << cnt << std::endl; 70 | out.close(); 71 | } 72 | video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true); 73 | return video; 74 | } 75 | 76 | bool checkReconnect(bool is_camera_connect) { 77 | if (!is_camera_connect) { 78 | int curr_gain = ((CameraWrapper* )video)->gain; 79 | int curr_exposure = ((CameraWrapper* )video)->exposure; 80 | delete video; 81 | video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/); 82 | is_camera_connect = video->init(); 83 | } 84 | return is_camera_connect; 85 | } 86 | 87 | auto video_writer = initVideoWriter(PROJECT_DIR"/video/"); 88 | 89 | void saveVideos(const cv::Mat &gimbal_src) { 90 | if (!gimbal_src.empty()) { 91 | video_writer.write(gimbal_src); 92 | } else return; 93 | } 94 | 95 | void showOrigin(const cv::Mat &src) { 96 | if (!src.empty()) { 97 | imshow("origin", src); 98 | cv::waitKey(1); 99 | } else return; 100 | } 101 | 102 | void extract(cv::Mat &src) {//图像预处理，将视频切成640×480的大小 103 | if (src.empty()) return; 104 | float length = static_cast(src.cols); 105 | float width = static_cast(src.rows); 106 | if (length / width > 640.0 / 480.0) { 107 | length *= 480.0 / width; 108 | resize(src, src, cv::Size(length, 480)); 109 | src = src(Rect((length - 640) / 2, 0, 640, 480)); 110 | } else { 111 | width *= 640.0 / length; 112 | resize(src, src, cv::Size(640, width)); 113 | src = src(Rect(0, (width - 480) / 2, 640, 480)); 114 | } 115 | } 116 | 117 | double getPointLength(const cv::Point2f &p) { 118 | return sqrt(p.x * p.x + p.y * p.y); 119 | } 120 | -------------------------------------------------------------------------------- /tools/TrainCNN/generate.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import cv2 4 | import random 5 | from tqdm import tqdm 6 | from forward import OUTPUT_NODES 7 | 8 | # 原图像行数 9 | SRC_ROWS = 36 10 | 11 | # 原图像列数 12 | SRC_COLS = 48 13 | 14 | # 原图像通道数 15 | SRC_CHANNELS = 3 16 | 17 | 18 | class DataSet: 19 | def __init__(self, folder): 20 | self.train_samples = [] 21 | self.train_labels = [] 22 | self.test_samples = [] 23 | self.test_labels = [] 24 | self.generate_data_sets(folder) 25 | 26 | def file2nparray(self, name): 27 | image = cv2.imread(name) 28 | image = cv2.resize(image, (SRC_COLS, SRC_ROWS)) 29 | image = image.astype(np.float32) 30 | return image / 255.0 31 | 32 | def id2label(self, id): 33 | a = np.zeros([OUTPUT_NODES]) 34 | a[id] = 1 35 | return a[:] 36 | 37 | def generate_data_sets(self, folder): 38 | sets = [] 39 | for i in range(OUTPUT_NODES): 40 | dir = "%s/id%d" % (folder, i) 41 | files = os.listdir(dir) 42 | for file in tqdm(files, postfix={"loading id": i}, dynamic_ncols=True): 43 | if file[-3:] == "jpg": 44 | sample = self.file2nparray("%s/%s" % (dir, file)) 45 | label = self.id2label(i) 46 | if random.random() < 0.7: 47 | self.train_samples.append(sample) 48 | self.train_labels.append(label) 49 | if i == 0: 50 | tmp = sample.copy() 51 | tmp = tmp[:, :, ::-1] 52 | self.train_samples.append(tmp) 53 | self.train_labels.append(label) 54 | else: 55 | tmp = sample.copy() 56 | tmp = 1.2 * tmp 57 | tmp = np.where(tmp > 1, 1, tmp) 58 | tmp = np.where(tmp < 0, 0, tmp) 59 | self.train_samples.append(tmp) 60 | self.train_labels.append(label) 61 | tmp = sample.copy() 62 | tmp = 0.8 * tmp 63 | tmp = np.where(tmp > 1, 1, tmp) 64 | tmp = np.where(tmp < 0, 0, tmp) 65 | self.train_samples.append(tmp) 66 | self.train_labels.append(label) 67 | else: 68 | self.test_samples.append(sample) 69 | self.test_labels.append(label) 70 | self.train_samples = np.array(self.train_samples) 71 | self.train_labels = np.array(self.train_labels) 72 | self.test_samples = np.array(self.test_samples) 73 | self.test_labels = np.array(self.test_labels) 74 | return sets 75 | 76 | def sample_train_sets(self, length, std=0.0): 77 | samples = [] 78 | labels = [] 79 | for i in range(length): 80 | id = random.randint(0, len(self.train_samples) - 1) 81 | samples.append(self.train_samples[id]) 82 | labels.append(self.train_labels[id]) 83 | samples = np.array(samples).copy() 84 | samples += np.random.normal(0, std, samples.shape) 85 | labels = np.array(labels) 86 | return samples, labels 87 | 88 | def sample_test_sets(self, length, std=0.0): 89 | samples = [] 90 | labels = [] 91 | for i in range(length): 92 | id = random.randint(0, len(self.test_samples) - 1) 93 | samples.append(self.test_samples[id]) 94 | labels.append(self.test_labels[id]) 95 | samples = np.array(samples).copy() 96 | samples += np.random.normal(0, std, samples.shape) 97 | labels = np.array(labels) 98 | return samples, labels 99 | 100 | def all_train_sets(self, std=0.0): 101 | return self.train_samples[:], self.train_labels[:] 102 | 103 | def all_test_sets(self, std=0.0): 104 | return self.test_samples[:], self.test_labels[:] 105 | -------------------------------------------------------------------------------- /tools/TrainCNN/grab.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | import mvsdk 3 | 4 | def main(): 5 | # 枚举相机 6 | DevList = mvsdk.CameraEnumerateDevice() 7 | nDev = len(DevList) 8 | if nDev < 1: 9 | print("No camera was found!") 10 | return 11 | 12 | for i, DevInfo in enumerate(DevList): 13 | print("{}: {} {}".format(i, DevInfo.GetFriendlyName(), DevInfo.GetPortType())) 14 | i = 0 if nDev == 1 else int(input("Select camera: ")) 15 | DevInfo = DevList[i] 16 | print(DevInfo) 17 | 18 | # 打开相机 19 | hCamera = 0 20 | try: 21 | hCamera = mvsdk.CameraInit(DevInfo, -1, -1) 22 | except mvsdk.CameraException as e: 23 | print("CameraInit Failed({}): {}".format(e.error_code, e.message) ) 24 | return 25 | 26 | # 获取相机特性描述 27 | cap = mvsdk.CameraGetCapability(hCamera) 28 | PrintCapbility(cap) 29 | 30 | # 判断是黑白相机还是彩色相机 31 | monoCamera = (cap.sIspCapacity.bMonoSensor != 0) 32 | 33 | # 黑白相机让ISP直接输出MONO数据，而不是扩展成R=G=B的24位灰度 34 | if monoCamera: 35 | mvsdk.CameraSetIspOutFormat(hCamera, mvsdk.CAMERA_MEDIA_TYPE_MONO8) 36 | 37 | # 相机模式切换成连续采集 38 | mvsdk.CameraSetTriggerMode(hCamera, 0) 39 | 40 | # 手动曝光，曝光时间30ms 41 | mvsdk.CameraSetAeState(hCamera, 0) 42 | mvsdk.CameraSetExposureTime(hCamera, 30 * 1000) 43 | 44 | # 让SDK内部取图线程开始工作 45 | mvsdk.CameraPlay(hCamera) 46 | 47 | # 计算RGB buffer所需的大小，这里直接按照相机的最大分辨率来分配 48 | FrameBufferSize = cap.sResolutionRange.iWidthMax * cap.sResolutionRange.iHeightMax * (1 if monoCamera else 3) 49 | 50 | # 分配RGB buffer，用来存放ISP输出的图像 51 | # 备注：从相机传输到PC端的是RAW数据，在PC端通过软件ISP转为RGB数据（如果是黑白相机就不需要转换格式，但是ISP还有其它处理，所以也需要分配这个buffer） 52 | pFrameBuffer = mvsdk.CameraAlignMalloc(FrameBufferSize, 16) 53 | 54 | # 从相机取一帧图片 55 | try: 56 | pRawData, FrameHead = mvsdk.CameraGetImageBuffer(hCamera, 2000) 57 | mvsdk.CameraImageProcess(hCamera, pRawData, pFrameBuffer, FrameHead) 58 | mvsdk.CameraReleaseImageBuffer(hCamera, pRawData) 59 | 60 | # 此时图片已经存储在pFrameBuffer中，对于彩色相机pFrameBuffer=RGB数据，黑白相机pFrameBuffer=8位灰度数据 61 | # 该示例中我们只是把图片保存到硬盘文件中 62 | status = mvsdk.CameraSaveImage(hCamera, "./grab.bmp", pFrameBuffer, FrameHead, mvsdk.FILE_BMP, 100) 63 | if status == mvsdk.CAMERA_STATUS_SUCCESS: 64 | print("Save image successfully. image_size = {}X{}".format(FrameHead.iWidth, FrameHead.iHeight) ) 65 | else: 66 | print("Save image failed. err={}".format(status) ) 67 | except mvsdk.CameraException as e: 68 | print("CameraGetImageBuffer failed({}): {}".format(e.error_code, e.message) ) 69 | 70 | # 关闭相机 71 | mvsdk.CameraUnInit(hCamera) 72 | 73 | # 释放帧缓存 74 | mvsdk.CameraAlignFree(pFrameBuffer) 75 | 76 | def PrintCapbility(cap): 77 | for i in range(cap.iTriggerDesc): 78 | desc = cap.pTriggerDesc[i] 79 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 80 | for i in range(cap.iImageSizeDesc): 81 | desc = cap.pImageSizeDesc[i] 82 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 83 | for i in range(cap.iClrTempDesc): 84 | desc = cap.pClrTempDesc[i] 85 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 86 | for i in range(cap.iMediaTypeDesc): 87 | desc = cap.pMediaTypeDesc[i] 88 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 89 | for i in range(cap.iFrameSpeedDesc): 90 | desc = cap.pFrameSpeedDesc[i] 91 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 92 | for i in range(cap.iPackLenDesc): 93 | desc = cap.pPackLenDesc[i] 94 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 95 | for i in range(cap.iPresetLut): 96 | desc = cap.pPresetLutDesc[i] 97 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 98 | for i in range(cap.iAeAlmSwDesc): 99 | desc = cap.pAeAlmSwDesc[i] 100 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 101 | for i in range(cap.iAeAlmHdDesc): 102 | desc = cap.pAeAlmHdDesc[i] 103 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 104 | for i in range(cap.iBayerDecAlmSwDesc): 105 | desc = cap.pBayerDecAlmSwDesc[i] 106 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 107 | for i in range(cap.iBayerDecAlmHdDesc): 108 | desc = cap.pBayerDecAlmHdDesc[i] 109 | print("{}: {}".format(desc.iIndex, desc.GetDescription()) ) 110 | 111 | main() 112 | -------------------------------------------------------------------------------- /others/src/options.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | bool show_armor_box = false; 11 | bool show_armor_boxes = false; 12 | bool show_light_blobs = false; 13 | bool show_origin = false; 14 | bool run_with_camera = false; 15 | bool save_video = false; 16 | bool wait_uart = false; 17 | bool save_labelled_boxes = false; 18 | bool show_process = false; 19 | bool show_energy = false; 20 | bool save_mark = false; 21 | bool show_info = false; 22 | bool run_by_frame = false; 23 | 24 | // 使用map保存所有选项及其描述和操作，加快查找速度。 25 | std::map> options = { 26 | {"--help",{ 27 | "show the help information.", [](){ 28 | LOG(LOG_MSG, ": " STR_CTR(WORD_BLUE, "All options below are for debug use.")); 29 | for(const auto &option : options){ 30 | LOG(LOG_MSG, ": " STR_CTR(WORD_GREEN, "%s: %s"), option.first.data(), option.second.first.data()); 31 | } 32 | } 33 | }}, 34 | {"--show-armor-box", { 35 | "show the aim box.", []() { 36 | show_armor_box = true; 37 | LOGM("Enable show armor box"); 38 | } 39 | }}, 40 | {"--show-armor-boxes",{ 41 | "show the candidate aim boxes.", [](){ 42 | show_armor_boxes = true; 43 | LOGM("Enable show armor boxes"); 44 | } 45 | }}, 46 | {"--show-light-blobs",{ 47 | "show the candidate light blobs.", [](){ 48 | show_light_blobs = true; 49 | LOGM("Enable show light blobs"); 50 | } 51 | }}, 52 | {"--show-origin", { 53 | "show the origin image.", [](){ 54 | show_origin = true; 55 | LOGM("Enable show origin"); 56 | } 57 | }}, 58 | {"--run-with-camera", { 59 | "start the program with camera directly without asking.", []() { 60 | run_with_camera = true; 61 | LOGM("Run with camera!"); 62 | } 63 | }}, 64 | {"--save-video", { 65 | "save the video.", [](){ 66 | save_video = true; 67 | LOGM("Enable save video!"); 68 | } 69 | }}, 70 | {"--save-labelled-boxes",{ 71 | "save the candidate boxes with their id labels.", [](){ 72 | save_labelled_boxes = true; 73 | LOGM("labelled armor boxes will be saved!"); 74 | } 75 | }}, 76 | {"--wait-uart", { 77 | "wait uart until ready before running.", [](){ 78 | wait_uart = true; 79 | LOGM("Enable wait uart!"); 80 | } 81 | }}, 82 | {"--run-by-frame",{ 83 | "run the code frame by frame.(normally used when run video)", [](){ 84 | run_by_frame = true; 85 | LOGM("Enable run frame by frame"); 86 | } 87 | }}, 88 | {"--show-process", { 89 | "", [](){ 90 | show_process = true; 91 | LOGM("Enable show processed image!"); 92 | } 93 | }}, 94 | {"--show-energy", { 95 | "",[](){ 96 | show_energy = true; 97 | LOGM("Enable show energy part!"); 98 | } 99 | }}, 100 | {"--save-mark", { 101 | "", [](){ 102 | save_mark = true; 103 | LOGM("Write down mark"); 104 | } 105 | }}, 106 | {"--show-info", { 107 | "", [](){ 108 | show_info = true; 109 | LOGM("Show information!"); 110 | } 111 | }}, 112 | {"--show-all", { 113 | "show all image windows.", [](){ 114 | show_armor_box = true; 115 | LOGM("Enable show armor box"); 116 | show_armor_boxes = true; 117 | LOGM("Enable show armor boxes"); 118 | show_light_blobs = true; 119 | LOGM("Enable show light blobs"); 120 | show_origin = true; 121 | LOGM("Enable show origin"); 122 | show_process = true; 123 | LOGM("Enable show processed image"); 124 | show_energy = true; 125 | LOGM("Enable show energy part"); 126 | } 127 | }} 128 | }; 129 | 130 | void processOptions(int argc, char **argv) { 131 | if (argc >= 2) { 132 | for (int i = 1; i < argc; i++) { 133 | auto key = options.find(std::string(argv[i])); // 寻找对应选项。 134 | if(key != options.end()){ 135 | key->second.second(); 136 | }else{ 137 | LOGW("Unknown option: %s. Use --help to see options.", argv[i]); 138 | } 139 | } 140 | } 141 | } -------------------------------------------------------------------------------- /energy/src/energy/calibrate/structing.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | 6 | using namespace cv; 7 | using std::cout; 8 | using std::endl; 9 | using std::vector; 10 | 11 | 12 | 13 | 14 | //---------------------------------------------------------------------------------------------------------------------- 15 | // 此函数对图像进行腐蚀与膨胀操作 16 | // --------------------------------------------------------------------------------------------------------------------- 17 | void Energy::FanStruct(cv::Mat &src) { 18 | Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); 19 | Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); 20 | Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); 21 | Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); 22 | Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); 23 | Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); 24 | 25 | dilate(src, src, element_dilate_1); 26 | erode(src,src, element_erode_1); 27 | erode(src,src, element_erode_2); 28 | erode(src,src, element_erode_3); 29 | } 30 | 31 | 32 | 33 | //---------------------------------------------------------------------------------------------------------------------- 34 | // 此函数对图像进行腐蚀与膨胀操作 35 | // --------------------------------------------------------------------------------------------------------------------- 36 | void Energy::ArmorStruct(cv::Mat &src) { 37 | Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); 38 | Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); 39 | Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); 40 | Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); 41 | Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); 42 | Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); 43 | 44 | dilate(src, src, element_dilate_1); 45 | erode(src,src, element_erode_1); 46 | } 47 | 48 | 49 | 50 | //---------------------------------------------------------------------------------------------------------------------- 51 | // 此函数对图像进行腐蚀与膨胀操作 52 | // --------------------------------------------------------------------------------------------------------------------- 53 | void Energy::FlowStripFanStruct(cv::Mat &src) { 54 | Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); 55 | Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); 56 | Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); 57 | Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); 58 | Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); 59 | Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); 60 | 61 | dilate(src, src, element_dilate_1); 62 | erode(src,src, element_erode_1); 63 | erode(src,src, element_erode_2); 64 | erode(src,src, element_erode_3); 65 | } 66 | 67 | 68 | 69 | //---------------------------------------------------------------------------------------------------------------------- 70 | // 此函数对图像进行腐蚀与膨胀操作 71 | // --------------------------------------------------------------------------------------------------------------------- 72 | void Energy::FlowStripStruct(cv::Mat &src) { 73 | Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); 74 | Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); 75 | Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); 76 | Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); 77 | Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); 78 | Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); 79 | 80 | dilate(src, src, element_dilate_1); 81 | erode(src,src, element_erode_1); 82 | erode(src,src, element_erode_2); 83 | erode(src,src, element_erode_3); 84 | } 85 | 86 | 87 | //---------------------------------------------------------------------------------------------------------------------- 88 | // 此函数对图像进行腐蚀与膨胀操作 89 | // --------------------------------------------------------------------------------------------------------------------- 90 | void Energy::CenterRStruct(cv::Mat &src) { 91 | Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(4, 4)); 92 | Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); 93 | Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); 94 | Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4)); 95 | 96 | erode(src,src, element_erode_1); 97 | dilate(src, src, element_dilate_1); 98 | } -------------------------------------------------------------------------------- /armor/src/armor_finder/armor_finder.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | /*===========================================================================*/ 6 | /* 使用本代码的兵种 */ 7 | /*===========================================================================*/ 8 | /* _______________ _______________ _______________ _______________ */ 9 | /* | _____ | | _ _ | | ____ | | _____ | */ 10 | /* || |___ / || || | || | || || | ___| || || |___ | || */ 11 | /* || |_ \ || || | || |_ || || |___ \ || || / / || */ 12 | /* || ___) | || || |__ _| || || ___) | || || / / || */ 13 | /* || |____/ || || |_| || || |____/ || || /_/ || */ 14 | /* |_______________| |_______________| |_______________| |_______________| */ 15 | /* */ 16 | /*===========================================================================*/ 17 | 18 | #define LOG_LEVEL LOG_NONE 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | std::map id2name = { //装甲板id到名称的map 26 | {-1, "OO"},{ 0, "NO"}, 27 | { 1, "B1"},{ 2, "B2"},{ 3, "B3"},{ 4, "B4"},{ 5, "B5"},{ 6, "B7"},{ 7, "B8"}, 28 | { 8, "R1"},{ 9, "R2"},{10, "R3"},{11, "R4"},{12, "R5"},{13, "R7"},{14, "R8"}, 29 | }; 30 | 31 | std::map name2id = { //装甲板名称到id的map 32 | {"OO", -1},{"NO", 0}, 33 | {"B1", 1},{"B2", 2},{"B3", 3},{"B4", 4},{"B5", 5},{"B7", 6},{"B8", 7}, 34 | {"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14}, 35 | }; 36 | 37 | std::map prior_blue = { 38 | {"B8", 0}, {"B1", 1}, {"B3", 2}, {"B4", 2}, {"B5", 2}, {"B7", 3}, {"B2", 4}, 39 | {"R8", 5}, {"R1", 6}, {"R3", 7}, {"R4", 7}, {"R5", 7}, {"R7", 8}, {"R2", 9}, 40 | {"NO", 10}, 41 | }; 42 | 43 | std::map prior_red = { 44 | {"R8", 0}, {"R1", 1}, {"R3", 2}, {"R4", 2}, {"R5", 2}, {"R7", 3}, {"R2", 4}, 45 | {"B8", 5}, {"B1", 6}, {"B3", 7}, {"B4", 7}, {"B5", 7}, {"B7", 8}, {"B2", 9}, 46 | {"NO", 10}, 47 | }; 48 | 49 | ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &anti_top) : 50 | serial(u), 51 | enemy_color(color), 52 | is_anti_top(anti_top), 53 | state(STANDBY_STATE), 54 | anti_switch_cnt(0), 55 | classifier(paras_folder), 56 | contour_area(0), 57 | tracking_cnt(0) { 58 | } 59 | 60 | void ArmorFinder::run(cv::Mat &src) { 61 | getsystime(frame_time); //　获取当前帧时间(不是足够精确) 62 | // stateSearchingTarget(src); // for debug 63 | // goto end; 64 | switch (state) { 65 | case SEARCHING_STATE: 66 | if (stateSearchingTarget(src)) { 67 | if ((target_box.rect & cv::Rect2d(0, 0, 640, 480)) == target_box.rect) { // 判断装甲板区域是否脱离图像区域 68 | if (!classifier) { /* 如果分类器不可用 */ 69 | cv::Mat roi = src(target_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */ 70 | cv::cvtColor(roi, roi_gray, CV_RGB2GRAY); 71 | cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY); 72 | contour_area = cv::countNonZero(roi_gray); 73 | } 74 | tracker = TrackerToUse::create(); // 成功搜寻到装甲板，创建tracker对象 75 | tracker->init(src, target_box.rect); 76 | state = TRACKING_STATE; 77 | tracking_cnt = 0; 78 | LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track")); 79 | } 80 | } 81 | break; 82 | case TRACKING_STATE: 83 | if (!stateTrackingTarget(src) || ++tracking_cnt > 100) { // 最多追踪100帧图像 84 | state = SEARCHING_STATE; 85 | LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!")); 86 | } 87 | break; 88 | case STANDBY_STATE: 89 | default: 90 | stateStandBy(); // currently meaningless 91 | } 92 | end: 93 | if(is_anti_top) { // 判断当前是否为反陀螺模式 94 | antiTop(); 95 | }else if(target_box.rect != cv::Rect2d()) { 96 | anti_top_cnt = 0; 97 | time_seq.clear(); 98 | angle_seq.clear(); 99 | sendBoxPosition(0); 100 | } 101 | 102 | if(target_box.rect != cv::Rect2d()){ 103 | last_box = target_box; 104 | } 105 | 106 | if (show_armor_box) { // 根据条件显示当前目标装甲板 107 | showArmorBox("box", src, target_box); 108 | cv::waitKey(1); 109 | } 110 | } 111 | 112 | -------------------------------------------------------------------------------- /energy/src/energy/tool/tool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace cv; 14 | 15 | 16 | 17 | //---------------------------------------------------------------------------------------------------------------------- 18 | // 此函数用于计算预测的击打点坐标 19 | // --------------------------------------------------------------------------------------------------------------------- 20 | void Energy::rotate(cv::Point target_point) { 21 | int x1, x2, y1, y2; 22 | // 为了减小强制转换的误差 23 | x1 = circle_center_point.x * 100; 24 | x2 = target_point.x * 100; 25 | y1 = circle_center_point.y * 100; 26 | y2 = target_point.y * 100; 27 | 28 | predict_point.x = static_cast( 29 | (x1 + (x2 - x1) * cos(-predict_rad * PI / 180.0) - (y1 - y2) * sin(-predict_rad * PI / 180.0)) / 100); 30 | predict_point.y = static_cast( 31 | (y1 - (x2 - x1) * sin(-predict_rad * PI / 180.0) - (y1 - y2) * cos(-predict_rad * PI / 180.0)) / 100); 32 | } 33 | 34 | 35 | 36 | 37 | //---------------------------------------------------------------------------------------------------------------------- 38 | // 此函数用于计算两点距离 39 | // --------------------------------------------------------------------------------------------------------------------- 40 | 41 | double Energy::pointDistance(cv::Point point_1, cv::Point point_2) { 42 | double distance = 0; 43 | distance = sqrt(pow(static_cast(point_1.x - point_2.x), 2) 44 | + pow(static_cast(point_1.y - point_2.y), 2)); 45 | return distance; 46 | } 47 | 48 | 49 | //---------------------------------------------------------------------------------------------------------------------- 50 | // 计算角度所处的极坐标分区，共五个 51 | // --------------------------------------------------------------------------------------------------------------------- 52 | int Energy::devide(float angle) { 53 | if (angle < 0)angle += 360;//若angle小于0，说明当前角度范围为-180°~180° 54 | return angle / 72.0; 55 | } 56 | 57 | 58 | //---------------------------------------------------------------------------------------------------------------------- 59 | // 计算直线上一点的横坐标 60 | // --------------------------------------------------------------------------------------------------------------------- 61 | int Energy::linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) { 62 | return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x; 63 | } 64 | 65 | 66 | 67 | //---------------------------------------------------------------------------------------------------------------------- 68 | // 计算旋转矩形内的两点占比 69 | // --------------------------------------------------------------------------------------------------------------------- 70 | double Energy::nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect) { 71 | int cnt = 0; 72 | cv::Point2f corners[4]; 73 | rotatedRect.points(corners); 74 | sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) { 75 | return p1.y < p2.y; 76 | }); 77 | for (int r = corners[0].y; r < corners[1].y; r++) { 78 | auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1; 79 | auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; 80 | if (start < 0)start = 0; 81 | if (end > 640)end = 640; 82 | for (int c = start; c < end; c++) { 83 | if (bin.at(r, c)) { 84 | cnt++; 85 | } 86 | } 87 | } 88 | for (int r = corners[1].y; r < corners[2].y; r++) { 89 | auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1; 90 | auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1; 91 | if (start < 0)start = 0; 92 | if (end > 640)end = 640; 93 | for (int c = start; c < end; c++) { 94 | if (bin.at(r, c)) { 95 | cnt++; 96 | } 97 | } 98 | } 99 | for (int r = corners[2].y; r < corners[3].y; r++) { 100 | auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1; 101 | auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; 102 | if (start < 0)start = 0; 103 | if (end > 640)end = 640; 104 | for (int c = start; c < end; c++) { 105 | if (bin.at(r, c)) { 106 | cnt++; 107 | } 108 | } 109 | } 110 | return double(cnt) / rotatedRect.size.area(); 111 | } -------------------------------------------------------------------------------- /armor/include/armor_finder/armor_finder.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-3-27. 3 | // 4 | 5 | #ifndef _ARMOR_FINDER_H_ 6 | #define _ARMOR_FINDER_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define BLOB_RED ENEMY_RED 18 | #define BLOB_BLUE ENEMY_BLUE 19 | 20 | #define BOX_RED ENEMY_RED 21 | #define BOX_BLUE ENEMY_BLUE 22 | 23 | #define IMAGE_CENTER_X (320) 24 | #define IMAGE_CENTER_Y (240-20) 25 | 26 | #define DISTANCE_HEIGHT_5MM (10700.0) // 单位: cm*pixel 27 | #define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM 28 | 29 | #define B1 1 30 | #define B2 2 31 | #define B3 3 32 | #define B4 4 33 | #define B5 5 34 | #define B7 6 35 | #define B8 7 36 | #define R1 8 37 | #define R2 9 38 | #define R3 10 39 | #define R4 11 40 | #define R5 12 41 | #define R7 13 42 | #define R8 14 43 | 44 | extern std::map id2name; //装甲板id到名称的map 45 | extern std::map name2id; //装甲板名称到id的map 46 | extern std::map prior_blue; 47 | extern std::map prior_red; 48 | 49 | 50 | /******************* 灯条类定义 ***********************/ 51 | class LightBlob { 52 | public: 53 | cv::RotatedRect rect; //灯条位置 54 | double area_ratio; 55 | double length; //灯条长度 56 | uint8_t blob_color; //灯条颜色 57 | 58 | LightBlob(cv::RotatedRect &r, double ratio, uint8_t color) : rect(r), area_ratio(ratio), blob_color(color) { 59 | length = max(rect.size.height, rect.size.width); 60 | }; 61 | LightBlob() = default; 62 | }; 63 | 64 | typedef std::vector LightBlobs; 65 | 66 | 67 | 68 | /******************* 装甲板类定义　**********************/ 69 | class ArmorBox{ 70 | public: 71 | typedef enum{ 72 | FRONT, SIDE, UNKNOWN 73 | } BoxOrientation; 74 | 75 | cv::Rect2d rect; 76 | LightBlobs light_blobs; 77 | uint8_t box_color; 78 | int id; 79 | 80 | explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0); 81 | 82 | cv::Point2f getCenter() const; // 获取装甲板中心 83 | double getBlobsDistance() const; // 获取两个灯条中心间距 84 | double lengthDistanceRatio() const; // 获取灯条中心距和灯条长度的比值 85 | double getBoxDistance() const; // 获取装甲板到摄像头的距离 86 | BoxOrientation getOrientation() const; // 获取装甲板朝向(误差较大，已弃用) 87 | 88 | bool operator<(const ArmorBox &box) const; // 装甲板优先级比较 89 | }; 90 | 91 | typedef std::vector ArmorBoxes; 92 | 93 | /********************* 自瞄类定义 **********************/ 94 | class ArmorFinder{ 95 | public: 96 | ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &anti_top); 97 | ~ArmorFinder() = default; 98 | 99 | private: 100 | typedef cv::TrackerKCF TrackerToUse; // Tracker类型定义 101 | 102 | typedef enum{ 103 | SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE 104 | } State; // 自瞄状态枚举定义 105 | 106 | systime frame_time; // 当前帧对应时间 107 | const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化 108 | const uint8_t &is_anti_top; // 进入反陀螺，引用外部变量，自动变化 109 | State state; // 自瞄状态对象实例 110 | ArmorBox target_box, last_box; // 目标装甲板 111 | int anti_switch_cnt; // 防止乱切目标计数器 112 | cv::Ptr tracker; // tracker对象实例 113 | Classifier classifier; // CNN分类器对象实例，用于数字识别 114 | int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） 115 | int tracking_cnt; // 记录追踪帧数，用于定时退出追踪 116 | Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 117 | systime last_front_time; // 上次陀螺正对时间 118 | int anti_top_cnt; 119 | RoundQueue top_periodms; // 陀螺周期循环队列 120 | vector time_seq; // 一个周期内的时间采样点 121 | vector angle_seq; // 一个周期内的角度采样点 122 | 123 | bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs); 124 | bool findArmorBox(const cv::Mat &src, ArmorBox &box); 125 | bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes); 126 | 127 | bool stateSearchingTarget(cv::Mat &src); // searching state主函数 128 | bool stateTrackingTarget(cv::Mat &src); // tracking state主函数 129 | bool stateStandBy(); // stand by state主函数（已弃用） 130 | 131 | void antiTop(); // 反小陀螺 132 | 133 | bool sendBoxPosition(uint16_t shoot); // 和主控板通讯 134 | public: 135 | void run(cv::Mat &src); // 自瞄主函数 136 | }; 137 | 138 | #endif /* _ARMOR_FINDER_H_ */ 139 | -------------------------------------------------------------------------------- /others/include/camera/camera_status.h: -------------------------------------------------------------------------------- 1 | #ifndef __CAMERA_STATUS_DEF__ 2 | #define __CAMERA_STATUS_DEF__ 3 | 4 | typedef int CameraSdkStatus; 5 | 6 | 7 | /*常用的宏*/ 8 | #define SDK_SUCCESS(_FUC_) (_FUC_ == CAMERA_STATUS_SUCCESS) 9 | 10 | #define SDK_UNSUCCESS(_FUC_) (_FUC_ != CAMERA_STATUS_SUCCESS) 11 | 12 | #define SDK_UNSUCCESS_RETURN(_FUC_,RET) if((RET = _FUC_) != CAMERA_STATUS_SUCCESS)\ 13 | {\ 14 | return RET;\ 15 | } 16 | 17 | #define SDK_UNSUCCESS_BREAK(_FUC_) if(_FUC_ != CAMERA_STATUS_SUCCESS)\ 18 | {\ 19 | break;\ 20 | } 21 | 22 | 23 | /* 常用错误 */ 24 | 25 | #define CAMERA_STATUS_SUCCESS 0 // 操作成功 26 | #define CAMERA_STATUS_FAILED -1 // 操作失败 27 | #define CAMERA_STATUS_INTERNAL_ERROR -2 // 内部错误 28 | #define CAMERA_STATUS_UNKNOW -3 // 未知错误 29 | #define CAMERA_STATUS_NOT_SUPPORTED -4 // 不支持该功能 30 | #define CAMERA_STATUS_NOT_INITIALIZED -5 // 初始化未完成 31 | #define CAMERA_STATUS_PARAMETER_INVALID -6 // 参数无效 32 | #define CAMERA_STATUS_PARAMETER_OUT_OF_BOUND -7 // 参数越界 33 | #define CAMERA_STATUS_UNENABLED -8 // 未使能 34 | #define CAMERA_STATUS_USER_CANCEL -9 // 用户手动取消了，比如roi面板点击取消，返回 35 | #define CAMERA_STATUS_PATH_NOT_FOUND -10 // 注册表中没有找到对应的路径 36 | #define CAMERA_STATUS_SIZE_DISMATCH -11 // 获得图像数据长度和定义的尺寸不匹配 37 | #define CAMERA_STATUS_TIME_OUT -12 // 超时错误 38 | #define CAMERA_STATUS_IO_ERROR -13 // 硬件IO错误 39 | #define CAMERA_STATUS_COMM_ERROR -14 // 通讯错误 40 | #define CAMERA_STATUS_BUS_ERROR -15 // 总线错误 41 | #define CAMERA_STATUS_NO_DEVICE_FOUND -16 // 没有发现设备 42 | #define CAMERA_STATUS_NO_LOGIC_DEVICE_FOUND -17 // 未找到逻辑设备 43 | #define CAMERA_STATUS_DEVICE_IS_OPENED -18 // 设备已经打开 44 | #define CAMERA_STATUS_DEVICE_IS_CLOSED -19 // 设备已经关闭 45 | #define CAMERA_STATUS_DEVICE_VEDIO_CLOSED -20 // 没有打开设备视频，调用录像相关的函数时，如果相机视频没有打开，则回返回该错误。 46 | #define CAMERA_STATUS_NO_MEMORY -21 // 没有足够系统内存 47 | #define CAMERA_STATUS_FILE_CREATE_FAILED -22 // 创建文件失败 48 | #define CAMERA_STATUS_FILE_INVALID -23 // 文件格式无效 49 | #define CAMERA_STATUS_WRITE_PROTECTED -24 // 写保护，不可写 50 | #define CAMERA_STATUS_GRAB_FAILED -25 // 数据采集失败 51 | #define CAMERA_STATUS_LOST_DATA -26 // 数据丢失，不完整 52 | #define CAMERA_STATUS_EOF_ERROR -27 // 未接收到帧结束符 53 | #define CAMERA_STATUS_BUSY -28 // 正忙(上一次操作还在进行中)，此次操作不能进行 54 | #define CAMERA_STATUS_WAIT -29 // 需要等待(进行操作的条件不成立)，可以再次尝试 55 | #define CAMERA_STATUS_IN_PROCESS -30 // 正在进行，已经被操作过 56 | #define CAMERA_STATUS_IIC_ERROR -31 // IIC传输错误 57 | #define CAMERA_STATUS_SPI_ERROR -32 // SPI传输错误 58 | #define CAMERA_STATUS_USB_CONTROL_ERROR -33 // USB控制传输错误 59 | #define CAMERA_STATUS_USB_BULK_ERROR -34 // USB BULK传输错误 60 | #define CAMERA_STATUS_SOCKET_INIT_ERROR -35 // 网络传输套件初始化失败 61 | #define CAMERA_STATUS_GIGE_FILTER_INIT_ERROR -36 // 网络相机内核过滤驱动初始化失败，请检查是否正确安装了驱动，或者重新安装。 62 | #define CAMERA_STATUS_NET_SEND_ERROR -37 // 网络数据发送错误 63 | #define CAMERA_STATUS_DEVICE_LOST -38 // 与网络相机失去连接，心跳检测超时 64 | #define CAMERA_STATUS_DATA_RECV_LESS -39 // 接收到的字节数比请求的少 65 | #define CAMERA_STATUS_FUNCTION_LOAD_FAILED -40 // 从文件中加载程序失败 66 | #define CAMERA_STATUS_CRITICAL_FILE_LOST -41 // 程序运行所必须的文件丢失。 67 | #define CAMERA_STATUS_SENSOR_ID_DISMATCH -42 // 固件和程序不匹配，原因是下载了错误的固件。 68 | #define CAMERA_STATUS_OUT_OF_RANGE -43 // 参数超出有效范围。 69 | #define CAMERA_STATUS_REGISTRY_ERROR -44 // 安装程序注册错误。请重新安装程序，或者运行安装目录Setup/Installer.exe 70 | #define CAMERA_STATUS_ACCESS_DENY -45 // 禁止访问。指定相机已经被其他程序占用时，再申请访问该相机，会返回该状态。(一个相机不能被多个程序同时访问) 71 | #define CAMERA_STATUS_CAMERA_NEED_RESET -46 // 表示相机需要复位后才能正常使用，此时请让相机断电重启，或者重启操作系统后，便可正常使用。 72 | 73 | 74 | 75 | //和AIA制定的标准相同 76 | /*#define CAMERA_AIA_SUCCESS 0x0000 */ 77 | #define CAMERA_AIA_PACKET_RESEND 0x0100 //该帧需要重传 78 | #define CAMERA_AIA_NOT_IMPLEMENTED 0x8001 //设备不支持的命令 79 | #define CAMERA_AIA_INVALID_PARAMETER 0x8002 //命令参数非法 80 | #define CAMERA_AIA_INVALID_ADDRESS 0x8003 //不可访问的地址 81 | #define CAMERA_AIA_WRITE_PROTECT 0x8004 //访问的对象不可写 82 | #define CAMERA_AIA_BAD_ALIGNMENT 0x8005 //访问的地址没有按照要求对齐 83 | #define CAMERA_AIA_ACCESS_DENIED 0x8006 //没有访问权限 84 | #define CAMERA_AIA_BUSY 0x8007 //命令正在处理中 85 | #define CAMERA_AIA_DEPRECATED 0x8008 //0x8008-0x0800B 0x800F 该指令已经废弃 86 | #define CAMERA_AIA_PACKET_UNAVAILABLE 0x800C //包无效 87 | #define CAMERA_AIA_DATA_OVERRUN 0x800D //数据溢出，通常是收到的数据比需要的多 88 | #define CAMERA_AIA_INVALID_HEADER 0x800E //数据包头部中某些区域与协议不匹配 89 | #define CAMERA_AIA_PACKET_NOT_YET_AVAILABLE 0x8010 //图像分包数据还未准备好，多用于触发模式，应用程序访问超时 90 | #define CAMERA_AIA_PACKET_AND_PREV_REMOVED_FROM_MEMORY 0x8011 //需要访问的分包已经不存在。多用于重传时数据已经不在缓冲区中 91 | #define CAMERA_AIA_PACKET_REMOVED_FROM_MEMORY 0x8012 //CAMERA_AIA_PACKET_AND_PREV_REMOVED_FROM_MEMORY 92 | #define CAMERA_AIA_NO_REF_TIME 0x0813 //没有参考时钟源。多用于时间同步的命令执行时 93 | #define CAMERA_AIA_PACKET_TEMPORARILY_UNAVAILABLE 0x0814 //由于信道带宽问题，当前分包暂时不可用，需稍后进行访问 94 | #define CAMERA_AIA_OVERFLOW 0x0815 //设备端数据溢出，通常是队列已满 95 | #define CAMERA_AIA_ACTION_LATE 0x0816 //命令执行已经超过有效的指定时间 96 | #define CAMERA_AIA_ERROR 0x8FFF //错误 97 | 98 | 99 | 100 | 101 | 102 | #endif -------------------------------------------------------------------------------- /armor/src/show_images/show_images.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace cv; 7 | 8 | 9 | void drawLightBlobs(cv::Mat &src, const LightBlobs &blobs){ 10 | for (const auto &blob:blobs) { 11 | Scalar color(0,255,0); 12 | if (blob.blob_color == BLOB_RED) 13 | color = Scalar(0, 0, 255); 14 | else if (blob.blob_color == BLOB_BLUE) 15 | color = Scalar(255, 0, 0); 16 | cv::Point2f vertices[4]; 17 | blob.rect.points(vertices); 18 | for (int j = 0; j < 4; j++) { 19 | cv::line(src, vertices[j], vertices[(j + 1) % 4], color, 2); 20 | } 21 | } 22 | } 23 | 24 | /************************** 25 | * 显示多个装甲板区域 * 26 | **************************/ 27 | void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes) { 28 | static Mat image2show; 29 | if (src.type() == CV_8UC1) {// 黑白图像 30 | cvtColor(src, image2show, COLOR_GRAY2RGB); 31 | } else if (src.type() == CV_8UC3) { //RGB 彩色 32 | image2show = src.clone(); 33 | } 34 | 35 | for (auto &box:armor_boxes) { 36 | if(box.box_color == BOX_BLUE) { 37 | rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); 38 | drawLightBlobs(image2show, box.light_blobs); 39 | }else if(box.box_color == BOX_RED){ 40 | rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); 41 | drawLightBlobs(image2show, box.light_blobs); 42 | } 43 | 44 | } 45 | imshow(windows_name, image2show); 46 | } 47 | 48 | /************************** 49 | * 显示多个装甲板区域及其类别 * 50 | **************************/ 51 | void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes) { 52 | static Mat image2show; 53 | if (src.type() == CV_8UC1) { // 黑白图像 54 | cvtColor(src, image2show, COLOR_GRAY2RGB); 55 | } else if (src.type() == CV_8UC3) { //RGB 彩色 56 | image2show = src.clone(); 57 | } 58 | for (const auto &box : boxes) { 59 | if(box.id != 0) { 60 | rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); 61 | drawLightBlobs(image2show, box.light_blobs); 62 | if (box.id == -1) 63 | putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 64 | Scalar(0, 255, 0)); 65 | else if (1 <= box.id && box.id < 8) 66 | putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 67 | Scalar(255, 0, 0)); 68 | else if (8 <= box.id && box.id < 15) 69 | putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 70 | Scalar(0, 0, 255)); 71 | else if (box.id != 0) 72 | LOGE_INFO("Invalid box id:%d!", box.id); 73 | } 74 | } 75 | imshow(window_names, image2show); 76 | 77 | } 78 | 79 | /************************** 80 | * 显示单个装甲板区域及其类别 * 81 | **************************/ 82 | void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &box) { 83 | static Mat image2show; 84 | if(box.rect == cv::Rect2d()){ 85 | imshow(windows_name, src); 86 | } 87 | if (src.type() == CV_8UC1) { // 黑白图像 88 | cvtColor(src, image2show, COLOR_GRAY2RGB); 89 | } else if (src.type() == CV_8UC3) { //RGB 彩色 90 | image2show = src.clone(); 91 | } 92 | drawLightBlobs(image2show, box.light_blobs); 93 | // static FILE *fp = fopen(PROJECT_DIR"/ratio.txt", "w"); 94 | // if(box.light_blobs.size() == 2) 95 | // fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.getBlobsDistance()) 96 | // cout << box.lengthDistanceRatio() << endl; 97 | 98 | if(box.getOrientation() == ArmorBox::FRONT){ 99 | rectangle(image2show, box.rect, Scalar(0, 255, 0), 3); 100 | }else{ 101 | rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); 102 | } 103 | 104 | char dist[10]; 105 | sprintf(dist, "%.1f", box.getBoxDistance()); 106 | if (box.id == -1) 107 | putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 108 | Scalar(0, 255, 0)); 109 | else if (1 <= box.id && box.id < 8) 110 | putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 111 | Scalar(255, 0, 0)); 112 | else if (8 <= box.id && box.id < 15) 113 | putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, 114 | Scalar(0, 0, 255)); 115 | else if (box.id != 0) 116 | LOGE_INFO("Invalid box id:%d!", box.id); 117 | imshow(windows_name, image2show); 118 | } 119 | 120 | /************************** 121 | * 显示多个灯条区域 * 122 | **************************/ 123 | void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs) { 124 | static Mat image2show; 125 | 126 | if (src.type() == CV_8UC1) { // 黑白图像 127 | cvtColor(src, image2show, COLOR_GRAY2RGB); 128 | } else if (src.type() == CV_8UC3) { //RGB 彩色 129 | image2show = src.clone(); 130 | } 131 | 132 | for (const auto &light_blob:light_blobs) { 133 | Scalar color(0, 255, 0); 134 | if (light_blob.blob_color == BLOB_RED) 135 | color = Scalar(0, 0, 255); 136 | else if (light_blob.blob_color == BLOB_BLUE) 137 | color = Scalar(255, 0, 0); 138 | cv::Point2f vertices[4]; 139 | light_blob.rect.points(vertices); 140 | for (int j = 0; j < 4; j++) { 141 | cv::line(image2show, vertices[j], vertices[(j + 1) % 4], color, 2); 142 | } 143 | } 144 | imshow(windows_name, image2show); 145 | } 146 | 147 | 148 | void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos){ 149 | static Mat image2show; 150 | if (src.type() == CV_8UC1) { // 黑白图像 151 | cvtColor(src, image2show, COLOR_GRAY2RGB); 152 | } else if (src.type() == CV_8UC3) { //RGB 彩色 153 | image2show = src.clone(); 154 | } 155 | rectangle(image2show, pos, Scalar(0, 255, 0), 1); 156 | imshow(window_names, image2show); 157 | } -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | /**********************************************************************/ 2 | /* ____ _ _____ _ _ ____ __ __ ______ __ */ 3 | /* / ___| | |_ _| | | | | _ \| \/ | / ___\ \ / / */ 4 | /* \___ \ _ | | | | | | | |_____| |_) | |\/| |_____| | \ \ / / */ 5 | /* ___) | |_| | | | | |_| |_____| _ <| | | |_____| |___ \ V / */ 6 | /* |____/ \___/ |_| \___/ |_| \_\_| |_| \____| \_/ */ 7 | /* */ 8 | /**********************************************************************/ 9 | // 本代码统一使用640×480大小的图像进行处理 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #define DO_NOT_CNT_TIME 25 | 26 | #include 27 | 28 | using namespace cv; 29 | using namespace std; 30 | 31 | McuData mcu_data = { // 单片机端回传结构体 32 | 0, // 当前云台yaw角 33 | 0, // 当前云台pitch角 34 | ARMOR_STATE, // 当前状态，自瞄-大符-小符 35 | 0, // 云台角度标记位 36 | 0, // 是否为反陀螺模式 37 | ENEMY_RED, // 敌方颜色 38 | 0, // 能量机关x轴补偿量 39 | 0, // 能量机关y轴补偿量 40 | }; 41 | 42 | WrapperHead *video = nullptr; // 云台摄像头视频源 43 | 44 | Serial serial(115200); // 串口对象 45 | uint8_t last_state = ARMOR_STATE; // 上次状态，用于初始化 46 | // 自瞄主程序对象 47 | ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top); 48 | // 能量机关主程序对象 49 | Energy energy(serial, mcu_data.enemy_color); 50 | 51 | int main(int argc, char *argv[]) { 52 | processOptions(argc, argv); // 处理命令行参数 53 | thread receive(uartReceive, &serial); // 开启串口接收线程 54 | 55 | int from_camera = 1; // 根据条件选择视频源 56 | if (!run_with_camera) { 57 | cout << "Input 1 for camera, 0 for video files" << endl; 58 | cin >> from_camera; 59 | } 60 | 61 | while (true) { 62 | // 打开视频源 63 | if (from_camera) { 64 | video = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2); 65 | } else { 66 | video = new VideoWrapper(PROJECT_DIR"/video/blue_big.avi"); 67 | } 68 | if (video->init()) { 69 | LOGM("video_source initialization successfully."); 70 | } else { 71 | LOGW("video_source unavailable!"); 72 | } 73 | 74 | // 跳过前10帧噪声图像。 75 | Mat src; 76 | for (int i = 0; i < 10; i++) { 77 | if (video) { 78 | video->read(src); 79 | } 80 | } 81 | bool ok = true; 82 | cout << "start running" << endl; 83 | do { 84 | char curr_state = mcu_data.state; 85 | CNT_TIME("Total", { 86 | if (curr_state != ARMOR_STATE) {//大能量机关模式 87 | if (last_state == ARMOR_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 88 | destroyAllWindows(); 89 | if (from_camera) { 90 | delete video; 91 | video = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2); 92 | if (video->init()) { 93 | LOGM("video_gimbal source initialization successfully."); 94 | } else { 95 | LOGW("video_gimbal source unavailable!"); 96 | } 97 | } 98 | if(curr_state == BIG_ENERGY_STATE){ 99 | energy.is_small = false; 100 | energy.is_big = true; 101 | LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!")); 102 | } else if (curr_state == SMALL_ENERGY_STATE){ 103 | energy.is_small = true; 104 | energy.is_big = false; 105 | LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); 106 | } 107 | energy.setEnergyInit(); 108 | } 109 | ok = checkReconnect(video->read(src)); 110 | #ifdef GIMBAL_FLIP_MODE 111 | flip(src, src, GIMBAL_FLIP_MODE); 112 | #endif 113 | if (!from_camera) extract(src); 114 | if (save_video) saveVideos(src);//保存视频 115 | if (show_origin) showOrigin(src);//显示原始图像 116 | energy.run(src); 117 | } else { // 自瞄模式 118 | if (last_state != ARMOR_STATE) { 119 | LOGM(STR_CTR(WORD_RED, "Start Armor!")); 120 | destroyAllWindows(); 121 | if (from_camera) { 122 | delete video; 123 | video = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); 124 | if (video->init()) { 125 | LOGM("video_gimbal source initialization successfully."); 126 | } else { 127 | LOGW("video_gimbal source unavailable!"); 128 | } 129 | } 130 | } 131 | CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { 132 | if(!checkReconnect(video->read(src))) continue; 133 | }); 134 | #ifdef GIMBAL_FLIP_MODE 135 | flip(src, src, GIMBAL_FLIP_MODE); 136 | #endif 137 | CNT_TIME("something whatever", { 138 | if (!from_camera) extract(src); 139 | if (save_video) saveVideos(src); 140 | if (show_origin) showOrigin(src); 141 | }); 142 | CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), { 143 | armor_finder.run(src); 144 | }); 145 | } 146 | last_state = curr_state;//更新上一帧状态 147 | if(run_by_frame) cv::waitKey(0); 148 | }); 149 | } while (ok); 150 | delete video; 151 | video = nullptr; 152 | cout << "Program fails. Restarting" << endl; 153 | } 154 | return 0; 155 | } 156 | 157 | -------------------------------------------------------------------------------- /energy/include/energy/energy.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #ifndef ENERGY_H 5 | #define ENERGY_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "energy/constant.h" 18 | #include "energy/param_struct_define.h" 19 | #include "serial.h" 20 | #include "additions.h" 21 | #include "options.h" 22 | 23 | using std::vector; 24 | 25 | class Energy { 26 | public: 27 | Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 28 | ~Energy();//默认析构函数 29 | 30 | bool is_big;//大符模式为true 31 | bool is_small;//小符模式为true 32 | 33 | void run(cv::Mat &src); 34 | 35 | Serial &serial;//串口 36 | void setEnergyInit();//设置能量机关初始化 37 | void sendEnergy();//发送能量机关数据 38 | void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据 39 | 40 | 41 | private: 42 | EnergyPartParam energy_part_param_;//能量机关的参数设置 43 | 44 | bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 45 | bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 46 | bool energy_mode_init;//大小符状态判断 47 | bool energy_rotation_init;//若仍在判断风车旋转方向，则为true 48 | bool start_guess;//进入猜测状态的标志 49 | bool change_target;//目标切换的标志 50 | 51 | uint8_t &ally_color;//我方颜色 52 | 53 | int curr_fps;//帧率 54 | int send_cnt;//向主控板发送的数据总次数 55 | int fans_cnt;//扇叶个数 56 | int last_fans_cnt;//上一帧的扇叶个数 57 | int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区 58 | int energy_rotation_direction;//风车旋转方向 59 | int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 60 | int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 61 | int last_mode;//上一帧的能量机关状态 62 | int manual_delta_x, manual_delta_y;//手动微调量 63 | int extra_delta_x, extra_delta_y;//在风车运动到最高点附近的额外补偿量 64 | 65 | float target_polar_angle;//待击打装甲板的极坐标角度 66 | float last_target_polar_angle_judge_change;//上一帧待击打装甲板的极坐标角度（用于判断目标切换） 67 | float last_target_polar_angle_judge_rotation;//上一帧待击打装甲板的极坐标角度（用于判断旋向） 68 | float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度 69 | float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度 70 | float predict_rad;//预测提前角 71 | float predict_rad_norm;//预测提前角的绝对值 72 | float attack_distance;//步兵与风车平面距离 73 | float center_delta_yaw, center_delta_pitch;//对心时相差的角度 74 | float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 75 | float shoot;//给主控板的指令，1表示跟随，2表示发射，3表示目标切换,4表示猜测模式 76 | float last_yaw, last_pitch;//PID中微分项 77 | float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项 78 | 79 | systime time_start_guess;//进入猜测模式的时间 80 | 81 | cv::RotatedRect centerR;//风车中心字母R的可能候选区 82 | cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个） 83 | cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个） 84 | cv::RotatedRect center_ROI;//风车中心候选区 85 | cv::RotatedRect target_armor;//目标装甲板（理论上仅一个） 86 | 87 | cv::Point circle_center_point;//风车圆心坐标 88 | cv::Point target_point;//目标装甲板中心坐标 89 | cv::Point guess_point; 90 | cv::Point predict_point;//预测的击打点坐标 91 | 92 | cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 93 | 94 | std::vector fans;//图像中所有扇叶 95 | std::vector armors;//图像中所有可能装甲板（可能存在误识别) 96 | std::vector flow_strip_fans;//可能的流动扇叶 97 | std::vector target_armors;//可能的目标装甲板 98 | std::vector flow_strips;//可能的流动条 99 | std::vector all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 100 | 101 | std::queue recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符 102 | 103 | 104 | void initEnergy();//能量机关初始化 105 | void initEnergyPartParam();//能量机关参数初始化 106 | void initRotation();//顺逆时针初始化 107 | 108 | void clearAll();//清空各vector 109 | void initImage(cv::Mat &src);//图像预处理 110 | 111 | bool stayGuessing();//保持在猜测模式 112 | 113 | int findFans(const cv::Mat &src);//寻找图中所有扇叶 114 | int findArmors(const cv::Mat &src);//寻找图中所有装甲板 115 | bool findCenterR(const cv::Mat &src);//寻找图中可能的风车中心字母R 116 | bool findFlowStrip(const cv::Mat &src);//寻找图中的流动条 117 | bool findCenterROI(const cv::Mat &src);//框取中心R候选区 118 | bool findFlowStripFan(const cv::Mat &src);//寻找图中的流动条所在扇叶 119 | bool findFlowStripWeak(const cv::Mat &src);//弱识别寻找图中的流动条 120 | bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板 121 | 122 | bool isValidFanContour(cv::Mat &src, const vector &fan_contour);//扇叶矩形尺寸要求 123 | bool isValidArmorContour(const vector &armor_contour);//装甲板矩形尺寸要求 124 | bool isValidCenterRContour(const vector ¢er_R_contour);//风车中心选区尺寸要求 125 | bool isValidFlowStripContour(const vector &flow_strip_contour);//流动条扇叶矩形尺寸要求 126 | bool isValidFlowStripFanContour(cv::Mat &src, const vector &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求 127 | 128 | void showFans(std::string windows_name, const cv::Mat &src);//显示扇叶 129 | void showArmors(std::string windows_name, const cv::Mat &src);//显示装甲板 130 | void showCenterR(std::string windows_name, const cv::Mat &src);//显示风车中心候选区R 131 | void showFlowStrip(std::string windows_name, const cv::Mat &src);//显示流动条 132 | void showFlowStripFan(std::string windows_name, const cv::Mat &src);//显示流动条所在扇叶 133 | void showGuessTarget(std::string windows_name, const cv::Mat &src);//显示猜测点位 134 | 135 | int devide(float angle);//将极坐标分为五个区域，判断一个角度处于哪个区域 136 | int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y);//计算直线上一点横坐标 137 | void rotate(cv::Point target_point);//获取预测点位 138 | double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离 139 | double nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比 140 | 141 | void writeDownMark(cv::Mat &src);//记录操作手的手动微调 142 | 143 | bool guessTarget();//获得猜测击打点位 144 | void changeTarget();//判断目标是否改变 145 | void getCenter();//对心 146 | void multipleMode(cv::Mat &src);//多模式切换 147 | void getTargetPolarAngle();//获得目标装甲板极坐标角度 148 | void getPredictPoint(cv::Point target_point);//获取预测点位 149 | void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位 150 | void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标 151 | 152 | void judgeMode();//判断大符还是小符 153 | void judgeShoot();//判断是否可以发弹 154 | bool isGuessingTimeout();//判断猜测模式是否超时（没打中） 155 | 156 | void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 157 | void imagePreprocess(cv::Mat &src);//图像通道分离 158 | 159 | void FanStruct(cv::Mat &src);//腐蚀和膨胀 160 | void ArmorStruct(cv::Mat &src);//腐蚀和膨胀 161 | void FlowStripFanStruct(cv::Mat &src);//腐蚀和膨胀 162 | void FlowStripStruct(cv::Mat &src);//腐蚀和膨胀 163 | void CenterRStruct(cv::Mat &src);//腐蚀和膨胀 164 | }; 165 | 166 | #endif //ENERGY_H 167 | 168 | -------------------------------------------------------------------------------- /armor/src/armor_finder/find/find_light_blobs.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-10. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | // 旋转矩形的长宽比 10 | static double lw_rate(const cv::RotatedRect &rect) { 11 | return rect.size.height > rect.size.width ? 12 | rect.size.height / rect.size.width : 13 | rect.size.width / rect.size.height; 14 | } 15 | // 轮廓面积和其最小外接矩形面积之比 16 | static double areaRatio(const std::vector &contour, const cv::RotatedRect &rect) { 17 | return cv::contourArea(contour) / rect.size.area(); 18 | } 19 | // 判断轮廓是否为一个灯条 20 | static bool isValidLightBlob(const std::vector &contour, const cv::RotatedRect &rect) { 21 | return (1.2 < lw_rate(rect) && lw_rate(rect) < 10) && 22 | // (rect.size.area() < 3000) && 23 | ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) || 24 | (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6)); 25 | } 26 | // 判断灯条颜色(此函数可以有性能优化). 27 | static uint8_t get_blob_color(const cv::Mat &src, const cv::RotatedRect &blobPos) { 28 | auto region = blobPos.boundingRect(); 29 | region.x -= fmax(3, region.width * 0.1); 30 | region.y -= fmax(3, region.height * 0.05); 31 | region.width += 2 * fmax(3, region.width * 0.1); 32 | region.height += 2 * fmax(3, region.height * 0.05); 33 | region &= cv::Rect(0, 0, src.cols, src.rows); 34 | cv::Mat roi = src(region); 35 | int red_cnt = 0, blue_cnt = 0; 36 | for (int row = 0; row < roi.rows; row++) { 37 | for (int col = 0; col < roi.cols; col++) { 38 | red_cnt += roi.at(row, col)[2]; 39 | blue_cnt += roi.at(row, col)[0]; 40 | } 41 | } 42 | if (red_cnt > blue_cnt) { 43 | return BLOB_RED; 44 | } else { 45 | return BLOB_BLUE; 46 | } 47 | } 48 | // 判断两个灯条区域是同一个灯条 49 | static bool isSameBlob(LightBlob blob1, LightBlob blob2) { 50 | auto dist = blob1.rect.center - blob2.rect.center; 51 | return (dist.x * dist.x + dist.y * dist.y) < 9; 52 | } 53 | // 开闭运算 54 | static void imagePreProcess(cv::Mat &src) { 55 | static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); 56 | erode(src, src, kernel_erode); 57 | 58 | static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); 59 | dilate(src, src, kernel_dilate); 60 | 61 | static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); 62 | dilate(src, src, kernel_dilate2); 63 | 64 | static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); 65 | erode(src, src, kernel_erode2); 66 | } 67 | // 在给定图像上寻找所有可能的灯条 68 | bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { 69 | cv::Mat color_channel; 70 | cv::Mat src_bin_light, src_bin_dim; 71 | std::vector channels; // 通道拆分 72 | 73 | cv::split(src, channels); /************************/ 74 | if (enemy_color == ENEMY_BLUE) { /* */ 75 | color_channel = channels[0]; /* 根据目标颜色进行通道提取 */ 76 | } else if (enemy_color == ENEMY_RED) { /* */ 77 | color_channel = channels[2]; /************************/ 78 | } 79 | 80 | int light_threshold; 81 | if(enemy_color == ENEMY_BLUE){ 82 | light_threshold = 225; 83 | }else{ 84 | light_threshold = 200; 85 | } 86 | cv::threshold(color_channel, src_bin_light, light_threshold, 255, CV_THRESH_BINARY); // 二值化对应通道 87 | if (src_bin_light.empty()) return false; 88 | imagePreProcess(src_bin_light); // 开闭运算 89 | 90 | cv::threshold(color_channel, src_bin_dim, 140, 255, CV_THRESH_BINARY); // 二值化对应通道 91 | if (src_bin_dim.empty()) return false; 92 | imagePreProcess(src_bin_dim); // 开闭运算 93 | 94 | if (src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) { 95 | imshow("bin_light", src_bin_light); 96 | imshow("bin_dim", src_bin_dim); 97 | } 98 | // 使用两个不同的二值化阈值同时进行灯条提取，减少环境光照对二值化这个操作的影响。 99 | // 同时剔除重复的灯条，剔除冗余计算，即对两次找出来的灯条取交集。 100 | std::vector> light_contours_light, light_contours_dim; 101 | LightBlobs light_blobs_light, light_blobs_dim; 102 | std::vector hierarchy_light, hierarchy_dim; 103 | cv::findContours(src_bin_light, light_contours_light, hierarchy_light, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); 104 | cv::findContours(src_bin_dim, light_contours_dim, hierarchy_dim, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); 105 | for (int i = 0; i < light_contours_light.size(); i++) { 106 | if (hierarchy_light[i][2] == -1) { 107 | cv::RotatedRect rect = cv::minAreaRect(light_contours_light[i]); 108 | if (isValidLightBlob(light_contours_light[i], rect)) { 109 | light_blobs_light.emplace_back( 110 | rect, areaRatio(light_contours_light[i], rect), get_blob_color(src, rect) 111 | ); 112 | } 113 | } 114 | } 115 | for (int i = 0; i < light_contours_dim.size(); i++) { 116 | if (hierarchy_dim[i][2] == -1) { 117 | cv::RotatedRect rect = cv::minAreaRect(light_contours_dim[i]); 118 | if (isValidLightBlob(light_contours_dim[i], rect)) { 119 | light_blobs_dim.emplace_back( 120 | rect, areaRatio(light_contours_dim[i], rect), get_blob_color(src, rect) 121 | ); 122 | } 123 | } 124 | } 125 | vector light_to_remove, dim_to_remove; 126 | for (int l = 0; l != light_blobs_light.size(); l++) { 127 | for (int d = 0; d != light_blobs_dim.size(); d++) { 128 | if (isSameBlob(light_blobs_light[l], light_blobs_dim[d])) { 129 | if (light_blobs_light[l].area_ratio > light_blobs_dim[d].area_ratio) { 130 | dim_to_remove.emplace_back(d); 131 | } else { 132 | light_to_remove.emplace_back(l); 133 | } 134 | } 135 | } 136 | } 137 | sort(light_to_remove.begin(), light_to_remove.end(), [](int a, int b) { return a > b; }); 138 | sort(dim_to_remove.begin(), dim_to_remove.end(), [](int a, int b) { return a > b; }); 139 | for (auto x : light_to_remove) { 140 | light_blobs_light.erase(light_blobs_light.begin() + x); 141 | } 142 | for (auto x : dim_to_remove) { 143 | light_blobs_dim.erase(light_blobs_dim.begin() + x); 144 | } 145 | for (const auto &light : light_blobs_light) { 146 | light_blobs.emplace_back(light); 147 | } 148 | for (const auto &dim : light_blobs_dim) { 149 | light_blobs.emplace_back(dim); 150 | } 151 | return light_blobs.size() >= 2; 152 | } 153 | -------------------------------------------------------------------------------- /energy/src/energy/show/show.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | 6 | using namespace cv; 7 | using std::cout; 8 | using std::endl; 9 | using std::vector; 10 | 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数用于显示图像中所有扇叶 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::showFans(std::string windows_name, const cv::Mat &src) { 17 | if (src.empty())return; 18 | static Mat image2show; 19 | 20 | if (src.type() == CV_8UC1) // 黑白图像 21 | { 22 | cvtColor(src, image2show, COLOR_GRAY2RGB); 23 | 24 | } else if (src.type() == CV_8UC3) //RGB 彩色 25 | { 26 | image2show = src.clone(); 27 | } 28 | for (const auto &fan : fans) { 29 | Point2f vertices[4]; //定义矩形的4个顶点 30 | fan.points(vertices); //计算矩形的4个顶点 31 | for (int i = 0; i < 4; i++) 32 | line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2); 33 | } 34 | imshow(windows_name, image2show); 35 | } 36 | 37 | 38 | //---------------------------------------------------------------------------------------------------------------------- 39 | // 此函数用于显示图像中所有装甲板 40 | // --------------------------------------------------------------------------------------------------------------------- 41 | void Energy::showArmors(std::string windows_name, const cv::Mat &src) { 42 | if (src.empty())return; 43 | static Mat image2show; 44 | 45 | if (src.type() == CV_8UC1) // 黑白图像 46 | { 47 | cvtColor(src, image2show, COLOR_GRAY2RGB); 48 | 49 | } else if (src.type() == CV_8UC3) //RGB 彩色 50 | { 51 | image2show = src.clone(); 52 | } 53 | for (const auto &armor : armors) { 54 | Point2f vertices[4]; //定义矩形的4个顶点 55 | armor.points(vertices); //计算矩形的4个顶点 56 | for (int i = 0; i < 4; i++) 57 | line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); 58 | } 59 | imshow(windows_name, image2show); 60 | } 61 | 62 | 63 | 64 | //---------------------------------------------------------------------------------------------------------------------- 65 | // 此函数用于显示图像中所有可能的风车中心候选区R 66 | // --------------------------------------------------------------------------------------------------------------------- 67 | void Energy::showCenterR(std::string windows_name, const cv::Mat &src) { 68 | if (src.empty())return; 69 | static Mat image2show; 70 | 71 | if (src.type() == CV_8UC1) // 黑白图像 72 | { 73 | cvtColor(src, image2show, COLOR_GRAY2RGB); 74 | 75 | } else if (src.type() == CV_8UC3) //RGB 彩色 76 | { 77 | image2show = src.clone(); 78 | } 79 | //cvtColor(image2show, image2show, COLOR_GRAY2RGB); 80 | Point2f vertices[4]; //定义矩形的4个顶点 81 | centerR.points(vertices); //计算矩形的4个顶点 82 | for (int i = 0; i < 4; i++) 83 | line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2); 84 | 85 | cv::circle(image2show, circle_center_point, 4, cv::Scalar(0, 0, 255), 2);//在图像中画出特征点，2是圆的半径 86 | 87 | imshow(windows_name, image2show); 88 | } 89 | 90 | 91 | //---------------------------------------------------------------------------------------------------------------------- 92 | // 此函数用于显示图像中流动条扇叶 93 | // --------------------------------------------------------------------------------------------------------------------- 94 | void Energy::showFlowStripFan(std::string windows_name, const cv::Mat &src) { 95 | if (src.empty())return; 96 | static Mat image2show; 97 | 98 | if (src.type() == CV_8UC1) // 黑白图像 99 | { 100 | cvtColor(src, image2show, COLOR_GRAY2RGB); 101 | 102 | } else if (src.type() == CV_8UC3) //RGB 彩色 103 | { 104 | image2show = src.clone(); 105 | } 106 | 107 | Point2f strip_fan_vertices[4]; //定义矩形的4个顶点 108 | flow_strip_fan.points(strip_fan_vertices); //计算矩形的4个顶点for (int i = 0; i < 4; i++) 109 | for (int i = 0; i < 4; i++) 110 | line(image2show, strip_fan_vertices[i], strip_fan_vertices[(i + 1) % 4], Scalar(127, 127, 255), 2); 111 | } 112 | 113 | //---------------------------------------------------------------------------------------------------------------------- 114 | // 此函数用于显示图像中流动条扇叶 115 | // --------------------------------------------------------------------------------------------------------------------- 116 | void Energy::showFlowStrip(std::string windows_name, const cv::Mat &src) { 117 | if (src.empty())return; 118 | static Mat image2show; 119 | 120 | if (src.type() == CV_8UC1) // 黑白图像 121 | { 122 | cvtColor(src, image2show, COLOR_GRAY2RGB); 123 | 124 | } else if (src.type() == CV_8UC3) //RGB 彩色 125 | { 126 | image2show = src.clone(); 127 | } 128 | 129 | Point2f strip_vertices[4]; //定义矩形的4个顶点 130 | flow_strip.points(strip_vertices); //计算矩形的4个顶点 131 | for (int i = 0; i < 4; i++) 132 | line(image2show, strip_vertices[i], strip_vertices[(i + 1) % 4], Scalar(0, 255, 0), 2); 133 | 134 | for (const auto &armor : armors) { 135 | if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) { 136 | Point2f vertices[4]; //定义矩形的4个顶点 137 | armor.points(vertices); //计算矩形的4个顶点 138 | for (int i = 0; i < 4; i++) 139 | line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2); 140 | } 141 | } 142 | 143 | Point2f ROI_vertices[4]; //定义矩形的4个顶点 144 | center_ROI.points(ROI_vertices); //计算矩形的4个顶点 145 | for (int i = 0; i < 4; i++) 146 | line(image2show, ROI_vertices[i], ROI_vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); 147 | imshow(windows_name, image2show); 148 | } 149 | 150 | 151 | //---------------------------------------------------------------------------------------------------------------------- 152 | // 此函数用于显示猜测的下一个目标点 153 | // --------------------------------------------------------------------------------------------------------------------- 154 | void Energy::showGuessTarget(std::string windows_name, const cv::Mat &src) { 155 | if (src.empty())return; 156 | static Mat image2show; 157 | 158 | if (src.type() == CV_8UC1) // 黑白图像 159 | { 160 | cvtColor(src, image2show, COLOR_GRAY2RGB); 161 | 162 | } else if (src.type() == CV_8UC3) //RGB 彩色 163 | { 164 | image2show = src.clone(); 165 | } 166 | cv::Point2f point = guess_point; 167 | cv::circle(image2show, point, 4, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 168 | 169 | imshow(windows_name, image2show); 170 | } 171 | 172 | 173 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 上海交通大学 RoboMaster 2019赛季 视觉代码 2 | 3 | 本代码是上海交通大学RoboMaster2019赛季步兵车辆的视觉部分，分为三个模块:**装甲板识别**，**能量机关**，以及**封装的设备驱动和配置文件**。可以提取能量机关以外的模块并修改main函数直接作为哨兵识别代码。 4 | 5 | 本代码统一使用**640×480**大小的图像进行处理 6 | 7 | | 作者 | 负责部分 | 微信号 | 8 | | ------ | -------------- | -------------------- | 9 | | 唐欣阳 | 自瞄装甲板识别 | xinyang_tang | 10 | | 卫志坤 | 自瞄装甲板识别 | | 11 | | 孙加桐 | 能量机关识别 | SJTxixiliadorabarryU | 12 | | 罗嘉鸣 | 能量机关识别 | | 13 | 14 | **如有BUG或者想交流的朋友欢迎积极联系我们** 15 | 16 | **分享部分比赛时摄像头录制的视频:** 17 | 18 | 链接: https://pan.baidu.com/s/1LwxEpeYYblX3cSzb59MTVg 提取码: 84ju 复制这段内容后打开百度网盘手机App，操作更方便哦 19 | 20 | --- 21 | 22 | 运行效果：自瞄帧率120（摄像头最大帧率）,识别距离根据环境不同大约8米左右(5mm焦距镜头)。 23 | 24 | ![front](https://github.com/lloi7/SJTU-RM-CV-2019/blob/master/picture0.png) 25 | ![side](https://github.com/lloi7/SJTU-RM-CV-2019/blob/master/picture1.png) 26 | 27 | ## 一、代码运行环境 28 | 29 | | 硬件设备 | 操作系统 | 运行库 | ToolChain | 30 | | ---------------------------------------------------- | -------------------------------------------- | ------------------------------------------------------------ | ---------------------------------------------------------- | 31 | | IntelNUC
MindVision工业相机×１
USB转TTL×１ | Ubuntu18.04
Ubuntu16.04
Ｗindows10 | OpenCV3.4.5
OpenCV_contrib3.4.5
Eigen3
MindVision相机驱动 | Ubuntu18/16 : cmake3+gcc7+g++7
Win10 : cmake3+VS2019 | 32 | 33 | MindVision相机型号：MV-UBS31GC 34 | 35 | **关于Windows环境下的运行支持，仅保证程序可以编译运行。对与部分辅助功能，如生成自启动脚本则不支持。** 36 | 37 | **实际装载在步兵和哨兵上的运行环境为Ubuntu18.04。** 38 | 39 | 相机驱动下载地址：[相机驱动](https://www.mindvision.com.cn) 40 | 41 | OpenCV下载地址：[OpenCV](https://github.com/opencv) 42 | 43 | OpenCV安装教程 : [linux](https://docs.opencv.org/3.4.5/d7/d9f/tutorial_linux_install.html) [Windows](https://docs.opencv.org/3.4.5/d3/d52/tutorial_windows_install.html) 44 | 45 | Eigen下载方法： 46 | * Ubuntu16/18: ```sudo apt install libeigen3-dev``` 47 | * Windows10 : [Eigen下载地址](http://eigen.tuxfamily.org/) 48 | 49 | ## 二、程序编译运行以及调试方式 50 | 51 | ### 1.编译运行 52 | 53 | * Ubuntu16/18（在项目文件夹下） 54 | 55 | ```shell 56 | mkdir build 57 | cd build 58 | cmake .. 59 | make -j8 60 | sudo ./run 61 | ``` 62 | 63 | * Windows10 64 | 65 | 打开cmake-gui，选择项目文件夹和build文件夹，生成VS工程。在VS中编译项目。 66 | 67 | ### 2.调试方式 68 | 69 | ```./run --help```可以查看所有命令行参数及其作用。所有调试选项都集成到了命令行参数中。 70 | 71 | **不使用任何参数直接运行将没有任何图像显示。** 72 | 73 | 需要调参的部分：主要需要根据车辆情况而调参的参数存放在others/include/config/setconfig.h中 74 | 75 | ### 3.工作条件 76 | 77 | * 对于自瞄，由于使用了数字识别，务必保证光照充足，图像上数字清晰可见。光照不足时，调整摄像头曝光或增益数值，直到数字清晰可见。 78 | * 务必保证摄像头焦距正确，同时镜片干净无污物。 79 | 80 | ## 三、文件目录结构 81 | ``` 82 | . 83 | ├── armor // 存放自瞄主要算法代码 84 | │ ├── include // 自瞄头文件 85 | │ └── src // 自瞄源码 86 | ├── CMakeLists.txt // cmake工程文件 87 | ├── energy // 存放能量机关主要算法代码 88 | │ ├── include // 能量机关头文件 89 | │ └── src // 能量机关源码 90 | ├── main.cpp // 主函数 91 | ├── others // 存放摄像头、串口、配置文件等 92 | │ ├── include // others头文件 93 | │ ├── libmvsdk.dylib // mac相机驱动链接库 94 | │ ├── libMVSDK.so // linux相机驱动链接库 95 | │ ├── MVCAMSDK_X64.dll // win10相机驱动链接库 96 | │ ├── MV-UB31-Group0.config // 相机配置文件 97 | │ └── src // others源码 98 | └── tools // 存放分类器训练代码及参数，自启动脚步等 99 | ├── auto-pull.sh // 自动代码更新脚本 100 | ├── create-startup.sh // 自启动文件创建脚本 101 | ├── monitor.bat // win10进程守护脚本 102 | ├── monitor.sh // linux进程守护脚本 103 | ├── para // 分类器参数 104 | └── TrainCNN // 分类器训练源码 105 | ``` 106 | ## 四、关键类解析 107 | 108 | | 类名 | 主要成员 | 主要接口 | 类的作用 | 109 | | --------------- | ------------------------------------------------------------ | ---------------------- | ------------------------------------------------------- | 110 | | ArmorFinder | 过多，不做赘述 | void run(cv::Mat &src) | 将一帧图像中装甲板的detection以及数据发送封装为一个类 | 111 | | Energy | 过多，不做赘述 | void run(cv::Mat &src) | 将一帧图像中能量机关的detection以及数据发送封装为一个类 | 112 | | EnergyPartParam | 过多，不做赘述 | 无 | 能量机关所有参数的集合 | 113 | | LightBlob | 灯条位置
灯条颜色 | 无 | 灯条类定义 | 114 | | ArmorBox | 装甲板位置
装甲板的两个灯条
装甲板颜色
装甲板数字id | 无 | 装甲板类定义 | 115 | 116 | ## 五、程序运行基本流程 117 | 118 | 　　　　　　　　　　　　↗ 大能量机关 ↘ 119 | 120 | 各项初始化→读取当前状态 → 小能量机关 → 回到读取状态 121 | 122 | 　　↓　　　　　　　　　↘　　自瞄　　↗ 123 | 124 | 数据接收线程 125 | 126 | ## 六、识别方式 127 | 128 | ### 1.自瞄装甲板识别方式 129 | 130 | ​ 首先对图像进行通道拆分以及二值化操作，再进行开闭运算，通过边缘提取和条件限制得出可能为灯条的部分。再对所有可能的灯条进行两两匹配，根据形状大小特性进行筛选，得出可能为装甲板的候选区。然后把所有候选区交给分类器判断，得出真实的装甲板及其数字id。最后根据优先级选取最终击打目标以及后续处理。 131 | ​ ![autoaim](https://github.com/lloi7/SJTU-RM-CV-2019/blob/master/自瞄流程图.png) 132 | ### 2.能量机关识别方式 133 | 134 | ​ 首先对图像进行二值化操作，然后进行一定腐蚀和膨胀，通过边缘提取和条件限制得出待击打叶片（锤子形）。在待击打叶片范围内进一步用类似方法寻找目标装甲板和流动条，在二者连线上寻找中心的“R”。根据目标装甲板坐标和中心坐标计算极坐标系下的目标角度，进而预测待击打点的坐标（小符为装甲板本身，大符需要旋转）。最后将待击打点坐标和图像中心的差值转换为yaw和pitch轴角度，增加一环PID后发送给云台主控板。 135 | 136 | ## 七、通信协议 137 | 138 | ### 1.通信方式 139 | 140 | 使用USB转TTL进行串口通信 141 | 142 | ### 2.数据接收结构体 143 | 144 | ```c++ 145 | struct McuData { 146 | float curr_yaw; // 当前云台yaw角度 147 | float curr_pitch; // 当前云台pitch角 148 | uint8_t state; // 当前状态，自瞄-大符-小符 149 | uint8_t mark; // 云台角度标记位 150 | uint8_t anti_top; // 是否为反陀螺模式 151 | uint8_t enemy_color; // 敌方颜色 152 | int delta_x; // 能量机关x轴补偿量 153 | int delta_y; // 能量机关y轴补偿量 154 | }; 155 | ``` 156 | 157 | 每个数据帧后使用字符```'\n'```作为帧尾标志 158 | 159 | ### 3.数据发送结构体 160 | 161 | ```c++ 162 | struct SendData { 163 | char start_flag; // 帧头标志，字符's' 164 | int16_t yaw; // float类型的实际角度(以度为单位)/100*(32768-1) 165 | int16_t pitch; // float类型的实际角度(以度为单位)/100*(32768-1) 166 | uint16_t shoot_delay; // 反陀螺模式下的发射延迟 167 | char end_flag; // 帧尾标识，字符'e' 168 | }; 169 | ``` 170 | 171 | 实际发送代码中没有使用这个结构体，而是使用uint8_t类型数组直接赋值代替 172 | 173 | ## 八、代码命名规范 174 | 175 | 函数名：使用首字母小写的驼峰命名法 176 | 177 | 类型名：使用首字母大写的驼峰命名法 178 | 179 | 变量名：使用下划线分割命名法 180 | 181 | ## 九、未来的优化方向 182 | * 基于灯条的候选区生成由于利用了灯条在其附近区域亮度是最高的这一先验知识，当摄像头曝光和增益较高，同时灯条旁边有其他较亮的物体(如反光、光晕过大等)，该灯条将无法被识别。反应在算法上即体现在二值化这一步骤之上。所以本项目采用多二值化阈值同时进行灯条提取最后进行合并的办法，有一定改善，但没有解决本质问题。**所以第一个优化方向便是环境适应力更强的灯条提取或者候选区提取**。 183 | * 代码中使用CNN分类器对灯条候选区进行筛选以及数字识别，但分类器总是不能做到100%的正确率，同时许多明显为背景的候选区有时也会被误判成装甲板，导致误识别。**所以第二个优化方向是更低的误识别率(可以从候选区生成入手或者从分类器入手)**。 184 | * 由于陀螺这样的机械设计基本上是强队的标准配置，所以我们也首次尝试了从视觉层面对陀螺有一个专门的击打方式，但实际应用场景较少(当前仅针对原地旋转的陀螺)，且对操作手不太友好(要求手动对准陀螺中心)，所以没有取得很大的实战价值。**所以第三个优化方向便是一个有着自动击打动态陀螺的系统**。 185 | 186 | --- 187 | **觉得对你有帮助请点个STAR哦:)** 188 | -------------------------------------------------------------------------------- /tools/para/conv1_w: -------------------------------------------------------------------------------- 1 | 3 2 | 6 3 | 5 4 | 5 5 | 0.026539411 6 | 0.42253798 7 | 0.6257618 8 | 0.7803654 9 | 0.56751746 10 | 0.26718912 11 | 0.7051625 12 | 0.71628624 13 | 0.7642506 14 | 0.49248695 15 | 0.16620474 16 | 0.3780306 17 | 0.7110501 18 | 0.6030068 19 | 0.4948883 20 | 0.24835615 21 | 0.33591226 22 | 0.43040946 23 | 0.5077497 24 | 0.5212336 25 | -0.027971355 26 | 0.2103128 27 | 0.25356093 28 | 0.07722227 29 | 0.18559895 30 | 0.32333145 31 | 0.345134 32 | 0.4201757 33 | 0.6501012 34 | 0.71342164 35 | 0.27166557 36 | 0.4905165 37 | 0.6236048 38 | 0.7930295 39 | 0.7500772 40 | 0.17779675 41 | 0.56028295 42 | 0.74555326 43 | 0.5372084 44 | 0.70782083 45 | 0.27072752 46 | 0.50543845 47 | 0.57856107 48 | 0.65362686 49 | 0.53758156 50 | 0.20370956 51 | 0.38661864 52 | 0.5690749 53 | 0.24994203 54 | 0.42308953 55 | -0.05610049 56 | 0.0263962 57 | 0.16409136 58 | -0.12595575 59 | -0.28413054 60 | -0.08711743 61 | 0.3854187 62 | 0.18076564 63 | 0.08114622 64 | -0.31865782 65 | -0.10122112 66 | 0.17201129 67 | 0.116371 68 | 0.13443075 69 | -0.3267171 70 | -0.23860133 71 | -0.092047565 72 | 0.09856403 73 | -0.15966126 74 | -0.42780057 75 | -0.27054727 76 | -0.17382775 77 | -0.19402348 78 | -0.50262624 79 | -0.6701677 80 | 0.021166094 81 | -0.15367271 82 | -0.3535248 83 | -0.3223209 84 | -0.3930027 85 | -0.012587458 86 | -0.2514576 87 | -0.028994415 88 | -0.23040394 89 | -0.22344537 90 | -0.28225404 91 | -0.12920779 92 | -0.084865615 93 | -0.079808 94 | -0.24189578 95 | -0.077354565 96 | -0.1364608 97 | -0.08271066 98 | 0.015865652 99 | -0.17308144 100 | -0.016932748 101 | -0.10400943 102 | -0.15847488 103 | -0.19929907 104 | -0.024590252 105 | -0.305961 106 | 0.41650155 107 | 0.71885103 108 | 0.5064972 109 | 0.2556136 110 | -0.2217093 111 | 0.3904318 112 | 0.679353 113 | 0.41345686 114 | 0.1750823 115 | -0.07011908 116 | 0.21723142 117 | 0.36451343 118 | 0.2734496 119 | -0.1827238 120 | -0.2640027 121 | -0.07665969 122 | -0.07281056 123 | -0.17140535 124 | -0.506126 125 | -0.39344314 126 | -0.19199198 127 | -0.43100312 128 | -0.5216478 129 | -0.8140914 130 | -0.22539687 131 | -0.1332064 132 | -0.18383063 133 | -0.3015942 134 | -0.3543749 135 | -0.07875634 136 | -0.046881627 137 | -0.17341466 138 | -0.17210038 139 | -0.16427462 140 | -0.12995729 141 | -0.09128473 142 | -0.07223676 143 | -0.05202489 144 | -0.17732152 145 | 0.032028172 146 | 0.022708599 147 | -0.07037333 148 | -0.24315825 149 | -0.3907585 150 | 0.09521279 151 | -0.07241904 152 | -0.15533188 153 | -0.030434519 154 | -0.262898 155 | 0.796653 156 | 0.9267637 157 | 0.93120253 158 | 0.9917096 159 | 0.9490124 160 | 0.8625081 161 | 1.0733354 162 | 1.1211755 163 | 1.1176518 164 | 0.9001717 165 | 0.7897956 166 | 1.1967996 167 | 1.1863062 168 | 1.0460231 169 | 0.86984694 170 | 0.7440283 171 | 0.88651806 172 | 0.9017338 173 | 0.81466335 174 | 0.6403854 175 | 0.50749934 176 | 0.5359783 177 | 0.54264694 178 | 0.5598802 179 | 0.41759288 180 | 0.21787402 181 | 0.42994195 182 | 0.5423997 183 | 0.46424487 184 | 0.4637137 185 | 0.4655036 186 | 0.5988346 187 | 0.5442244 188 | 0.71659744 189 | 0.6306643 190 | 0.52392393 191 | 0.66216165 192 | 0.57559747 193 | 0.50626665 194 | 0.53091234 195 | 0.50743634 196 | 0.57716715 197 | 0.7824497 198 | 0.41367468 199 | 0.55546504 200 | 0.3030582 201 | 0.6136134 202 | 0.36521676 203 | 0.43919942 204 | 0.33232304 205 | 0.51073956 206 | 0.9655301 207 | 1.0148821 208 | 0.8726425 209 | 0.30961013 210 | 0.747004 211 | 1.1461548 212 | 1.3142052 213 | 0.889127 214 | 0.45464084 215 | 0.5808323 216 | 1.0770975 217 | 1.3939735 218 | 0.9165097 219 | 0.6586972 220 | 0.57355326 221 | 0.9547619 222 | 0.9447146 223 | 0.66483366 224 | 0.30041847 225 | 0.3404827 226 | 0.5929992 227 | 0.7068603 228 | 0.41881943 229 | -0.11529228 230 | -0.06013718 231 | 0.029292889 232 | 0.2176899 233 | 0.14616063 234 | 0.14816451 235 | 0.13036805 236 | 0.1793373 237 | 0.3953 238 | 0.51387286 239 | 0.35285914 240 | 0.06709124 241 | 0.2171802 242 | 0.6583215 243 | 0.5220034 244 | 0.44539294 245 | 0.054360673 246 | 0.18225458 247 | 0.47989392 248 | 0.5178179 249 | 0.4576618 250 | 0.16889913 251 | 0.27844474 252 | 0.32638916 253 | 0.39993882 254 | 0.119894765 255 | 0.67456526 256 | 1.4996389 257 | 1.7681378 258 | 1.6962894 259 | 1.266272 260 | 0.73875403 261 | 1.6141837 262 | 1.7386745 263 | 1.671539 264 | 1.1571932 265 | 0.546496 266 | 1.1523583 267 | 1.2499596 268 | 1.2602416 269 | 0.7421503 270 | 0.19625238 271 | 0.6587611 272 | 0.66948915 273 | 0.43528196 274 | -0.008682725 275 | 0.05109705 276 | 0.13888682 277 | 0.24387836 278 | -0.13002548 279 | -0.3703551 280 | 0.06102093 281 | 0.0076322197 282 | -0.035532437 283 | -0.029681634 284 | -0.057815406 285 | 0.20116982 286 | 0.27798048 287 | 0.22556874 288 | 0.3058461 289 | 0.039763346 290 | 0.10804575 291 | 0.42170766 292 | 0.3397586 293 | 0.44702667 294 | 0.19944976 295 | 0.12492277 296 | 0.2690411 297 | 0.31178012 298 | 0.307435 299 | 0.09525637 300 | 0.31001845 301 | 0.20959602 302 | 0.30736595 303 | 0.2639023 304 | 0.07473994 305 | -0.20428133 306 | 0.123462535 307 | 0.08172282 308 | 0.0273528 309 | 0.012557347 310 | -0.050085112 311 | 0.15972462 312 | 0.4576377 313 | 0.26537335 314 | 0.05294297 315 | 0.1920879 316 | 0.43903905 317 | 0.44806978 318 | 0.24177204 319 | 0.15497287 320 | 0.004198166 321 | 0.24119955 322 | 0.23940569 323 | 0.30403528 324 | 0.10443368 325 | -0.03242823 326 | -0.009877937 327 | -0.056617204 328 | -0.034309167 329 | -0.30718032 330 | -0.21831672 331 | -0.33697575 332 | -0.06903099 333 | -0.21208347 334 | -0.26106125 335 | -0.03470626 336 | -0.17156434 337 | -0.034266062 338 | -0.11911982 339 | -0.06517054 340 | -0.0014468514 341 | 0.13053921 342 | -0.021993743 343 | 0.08158244 344 | -0.048275527 345 | 0.073179156 346 | 0.11620164 347 | 0.18908599 348 | 0.090895034 349 | -0.13516097 350 | -0.041694365 351 | 0.19145414 352 | 0.06233252 353 | -0.015241079 354 | -0.08720653 355 | 0.27016148 356 | 0.68275934 357 | 0.9259101 358 | 0.87553996 359 | 0.6730488 360 | 0.5436024 361 | 0.93307287 362 | 1.284303 363 | 1.215596 364 | 0.7024047 365 | 0.6535529 366 | 1.0312322 367 | 1.2054113 368 | 1.1569601 369 | 0.78507113 370 | 0.46270427 371 | 0.96070755 372 | 1.0094548 373 | 0.8725796 374 | 0.584176 375 | 0.2608554 376 | 0.68085426 377 | 0.6351991 378 | 0.5974386 379 | 0.44071475 380 | -0.14620513 381 | -0.051413815 382 | 0.123140134 383 | 0.3022954 384 | 0.3937261 385 | 0.14715014 386 | 0.14558095 387 | 0.37810993 388 | 0.6081454 389 | 0.4194721 390 | -0.048754267 391 | 0.08287452 392 | 0.5189599 393 | 0.62214893 394 | 0.7360417 395 | -0.06856284 396 | 0.1070881 397 | 0.296386 398 | 0.6012432 399 | 0.60254496 400 | -0.24324551 401 | 0.14447688 402 | 0.18799835 403 | 0.46327543 404 | 0.45924807 405 | 0.20411673 406 | 1.1002504 407 | 1.4868475 408 | 1.5346047 409 | 1.0632775 410 | 0.33373544 411 | 1.1446538 412 | 1.4786532 413 | 1.4592378 414 | 0.9056299 415 | 0.22967416 416 | 1.0249681 417 | 1.2026587 418 | 1.0556481 419 | 0.6419271 420 | 0.16762647 421 | 0.475342 422 | 0.64084166 423 | 0.45603147 424 | 0.19102122 425 | -0.17340069 426 | 0.14535804 427 | 0.13595402 428 | 0.09039172 429 | -0.185116 430 | 0.11963739 431 | 0.3589415 432 | 0.3841168 433 | 0.5203414 434 | 0.20129839 435 | 0.20421675 436 | 0.22073998 437 | 0.4301539 438 | 0.6149839 439 | 0.42226243 440 | 0.31898078 441 | 0.31484243 442 | 0.34544006 443 | 0.5597543 444 | 0.4067382 445 | 0.12620181 446 | 0.39053708 447 | 0.38691083 448 | 0.5219121 449 | 0.62844783 450 | 0.3108787 451 | 0.46809235 452 | 0.39197984 453 | 0.38944077 454 | 0.4951729 455 | -------------------------------------------------------------------------------- /energy/src/energy/clear/energy_init.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xixiliadorabarry on 1/24/19. 3 | // 4 | #include "energy/energy.h" 5 | #include "log.h" 6 | 7 | using namespace cv; 8 | using std::cout; 9 | using std::endl; 10 | using std::vector; 11 | 12 | 13 | //---------------------------------------------------------------------------------------------------------------------- 14 | // 此函数对能量机关成员变量进行初始化 15 | // --------------------------------------------------------------------------------------------------------------------- 16 | void Energy::initEnergy() { 17 | is_guessing = false; 18 | is_predicting = true; 19 | energy_mode_init = true; 20 | energy_rotation_init = true; 21 | start_guess = false; 22 | change_target = false; 23 | 24 | curr_fps = 0; 25 | send_cnt = 0; 26 | fans_cnt = 0; 27 | last_fans_cnt = 0; 28 | guess_devide = 0; 29 | energy_rotation_direction = ANTICLOCKWISE; 30 | clockwise_rotation_init_cnt = 0; 31 | anticlockwise_rotation_init_cnt = 0; 32 | last_mode = -1;//既不是大符也不是小符 33 | manual_delta_x = 0; 34 | manual_delta_y = 0; 35 | extra_delta_y = 0; 36 | extra_delta_x = 0; 37 | 38 | target_polar_angle = -1000; 39 | last_target_polar_angle_judge_change = -1000; 40 | last_target_polar_angle_judge_rotation = -1000; 41 | guess_polar_angle = -1000; 42 | last_base_angle = -1000; 43 | predict_rad = 0; 44 | predict_rad_norm = 25; 45 | attack_distance = ATTACK_DISTANCE; 46 | center_delta_yaw = 1000; 47 | center_delta_pitch = 1000; 48 | yaw_rotation = 0; 49 | pitch_rotation = 0; 50 | shoot = 0; 51 | last_yaw = 0; 52 | last_pitch = 0; 53 | sum_yaw = 0; 54 | sum_pitch = 0; 55 | 56 | circle_center_point = Point(0, 0); 57 | target_point = Point(0, 0); 58 | guess_point = Point(0, 0); 59 | predict_point = Point(0, 0); 60 | 61 | fans.clear(); 62 | armors.clear(); 63 | flow_strip_fans.clear(); 64 | target_armors.clear(); 65 | flow_strips.clear(); 66 | all_target_armor_centers.clear(); 67 | while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop(); 68 | 69 | } 70 | 71 | 72 | //---------------------------------------------------------------------------------------------------------------------- 73 | // 此函数对能量机关参数进行初始化 74 | // --------------------------------------------------------------------------------------------------------------------- 75 | void Energy::initEnergyPartParam() { 76 | energy_part_param_.RED_GRAY_THRESH = 180;//game 77 | energy_part_param_.BLUE_GRAY_THRESH = 100;//game 78 | energy_part_param_.SPLIT_GRAY_THRESH = 180; 79 | 80 | energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000; 81 | energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500; 82 | energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45; 83 | energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; 84 | energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10; 85 | energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; 86 | energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5; 87 | energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2; 88 | energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6; 89 | 90 | energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500; 91 | energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180; 92 | energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10; 93 | energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50; 94 | energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0; 95 | energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30; 96 | energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; 97 | energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; 98 | 99 | energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200; 100 | energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40; 101 | energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6; 102 | energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20; 103 | energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6; 104 | energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20; 105 | energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2; 106 | energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; 107 | energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6; 108 | energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; 109 | 110 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000; 111 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500; 112 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; 113 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; 114 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; 115 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52; 116 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8; 117 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2; 118 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58; 119 | energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34; 120 | 121 | energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700; 122 | energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100; 123 | energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32; 124 | energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55; 125 | energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4; 126 | energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; 127 | energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7; 128 | energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; 129 | energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6; 130 | energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; 131 | 132 | energy_part_param_.TWIN_ANGEL_MAX = 10; 133 | energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40; 134 | 135 | energy_part_param_.TWIN_POINT_MAX = 20; 136 | 137 | energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28; 138 | energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52; 139 | } 140 | 141 | 142 | //---------------------------------------------------------------------------------------------------------------------- 143 | // 此函数对能量机关旋转方向进行初始化 144 | // --------------------------------------------------------------------------------------------------------------------- 145 | void Energy::initRotation() { 146 | if (target_polar_angle >= -180 && last_target_polar_angle_judge_rotation >= -180 147 | && fabs(target_polar_angle - last_target_polar_angle_judge_rotation) < 30) { 148 | //target_polar_angle和last_target_polar_angle_judge_rotation的初值均为1000，大于-180表示刚开始几帧不要 149 | //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断 150 | if (target_polar_angle < last_target_polar_angle_judge_rotation) clockwise_rotation_init_cnt++; 151 | else if (target_polar_angle > last_target_polar_angle_judge_rotation) anticlockwise_rotation_init_cnt++; 152 | } 153 | //由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针 154 | if (clockwise_rotation_init_cnt == 15) { 155 | energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针 156 | cout << "rotation: " << energy_rotation_direction << endl; 157 | energy_rotation_init = false; 158 | } else if (anticlockwise_rotation_init_cnt == 15) { 159 | energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针 160 | cout << "rotation: " << energy_rotation_direction << endl; 161 | energy_rotation_init = false; 162 | } 163 | last_target_polar_angle_judge_rotation = target_polar_angle; 164 | } 165 | -------------------------------------------------------------------------------- /others/include/log.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-2-19. 3 | // 4 | // 该文件提供一个更加方便的调试信息输出方式 5 | // 所有输出信息分为三个LEVEL:MSG,WARNING,ERROR 6 | // 可使用宏LOG_LEVEL定义当前文件使用的输出LEVEL 7 | // 高于该LEVEL的输出讲不会被显示 8 | // ============================================================ 9 | // 输出API： 10 | // LOG(level, format, ...) 11 | // arguments: level:当前输出的level 12 | // format:标准printf格式化字符串 13 | // LOGM(format, ...) 使用MSG level进行输出 14 | // LOGW(format, ...) 使用WARNING level进行输出 15 | // LOGE(format, ...) 使用ERROR level进行输出 16 | // ============================================================ 17 | // 输出颜色API：(仅对部分终端生效) 18 | // STR_CTR(ctrs, str) 19 | // arguments: ctrs:该字符串对应的颜色(所有以WORD开头的宏) 20 | //　　　　　　　str:需要上色的字符串 21 | // ============================================================ 22 | // 时间计算API：(需要配合systime.h使用) 23 | // CNT_TIME(tag, codes, ...) 24 | // arguments: tag:显示代码块执行时间前的用户信息，支持printf格式化字符串 25 | // codes:需要被统计时间的代码块 26 | // attention: 代码块内定义的局部变量作用域仅限于该代码块 27 | // 代码块内不支持使用break,continue语句,将无法达到预想效果 28 | // 支持多出口离开代码块都能显示代码块执行时间 29 | // 30 | #ifndef _LOG_H_ 31 | #define _LOG_H_ 32 | 33 | #include 34 | #include 35 | 36 | /************** Define the control code *************/ 37 | #define START_CTR "\033[0" 38 | #define END_CTR "m" 39 | #define CLEAR_CODE ";0" 40 | #define LIGHT_CODE ";1" 41 | #define LINE_CODE ";4" 42 | #define BLINK_CODE ";5" 43 | #define REVERSE_CODE ";7" 44 | #define VANISH_CODE ";8" 45 | #define WORD_WHITE_CODE ";30" 46 | #define WORD_RED_CODE ";31" 47 | #define WORD_GREEN_CODE ";32" 48 | #define WORD_YELLOW_CODE ";33" 49 | #define WORD_BLUE_CODE ";34" 50 | #define WORD_PURPLE_CODE ";35" 51 | #define WORD_CYAN_CODE ";36" 52 | #define WORD_GRAY_CODE ";37" 53 | #define BACK_WHITE_CODE ";40" 54 | #define BACK_RED_CODE ";41" 55 | #define BACK_GREEN_CODE ";42" 56 | #define BACK_YELLOW_CODE ";43" 57 | #define BACK_BLUE_CODE ";44" 58 | #define BACK_PURPLE_CODE ";45" 59 | #define BACK_CYAN_CODE ";46" 60 | #define BACK_GRAY_CODE ";47" 61 | 62 | #define CTRS(ctrs) START_CTR ctrs END_CTR 63 | #define STR_CTR(ctrs, str) START_CTR ctrs END_CTR str CLEAR_ALL 64 | 65 | #define WORD_WHITE WORD_WHITE_CODE 66 | #define WORD_RED WORD_RED_CODE 67 | #define WORD_GREEN WORD_GREEN_CODE 68 | #define WORD_YELLOW WORD_YELLOW_CODE 69 | #define WORD_BLUE WORD_BLUE_CODE 70 | #define WORD_PURPLE WORD_PURPLE_CODE 71 | #define WORD_CYAN WORD_CYAN_CODE 72 | #define WORD_GRAY WORD_GRAY_CODE 73 | #define WORD_LIGHT_WHITE LIGHT_CODE WORD_WHITE 74 | #define WORD_LIGHT_RED LIGHT_CODE WORD_RED 75 | #define WORD_LIGHT_GREEN LIGHT_CODE WORD_GREEN 76 | #define WORD_LIGHT_YELLOW LIGHT_CODE WORD_YELLOW 77 | #define WORD_LIGHT_BLUE LIGHT_CODE WORD_BLUE 78 | #define WORD_LIGHT_PURPLE LIGHT_CODE WORD_PURPLE 79 | #define WORD_LIGHT_CYAN LIGHT_CODE WORD_CYAN 80 | #define WORD_LIGHT_GRAY LIGHT_CODE WORD_GRAY 81 | #define CLEAR_ALL CTRS(CLEAR_CODE) 82 | /*************** Define the log level value ***************/ 83 | #define LOG_NONE 0 84 | #define LOG_ERROR 1 85 | #define LOG_WARNING 2 86 | #define LOG_MSG 3 87 | #define LOG_NOTHING 4 88 | /************** Ensure the current log level **************/ 89 | #ifndef LOG_LEVEL 90 | #define LOG_LEVEL LOG_MSG 91 | #endif 92 | #if LOG_LEVEL < LOG_NONE 93 | #define LOG_LEVEL LOG_NONE 94 | #elif LOG_LEVEL > LOG_MSG 95 | #define LOG_LEVEL LOG_MSG 96 | #endif 97 | /******* Ensure the color corresponding to the level ******/ 98 | #ifndef LOG_ERROR_COLOR 99 | #define LOG_ERROR_COLOR WORD_RED 100 | #endif 101 | #ifndef LOG_WARNING_COLOR 102 | #define LOG_WARNING_COLOR WORD_YELLOW 103 | #endif 104 | #ifndef LOG_MSG_COLOR 105 | #define LOG_MSG_COLOR WORD_GRAY 106 | #endif 107 | #ifndef LOG_LINK_COLOR 108 | #define LOG_LINK_COLOR LINE_CODE WORD_BLUE 109 | #endif 110 | 111 | /********************** log place *************************/ 112 | #ifndef LOG_OUT 113 | #define LOG_OUT stdout 114 | #endif 115 | /******************** The log API *************************/ 116 | #define LOG_0(format, ...) fprintf(LOG_OUT, format, ##__VA_ARGS__) 117 | #if LOG_LEVEL >= LOG_ERROR 118 | #define LOG_1(format, ...) fprintf(LOG_OUT, format, ##__VA_ARGS__) 119 | #else 120 | #define LOG_1(format, ...) ((void)0) 121 | #endif 122 | #if LOG_LEVEL >= LOG_WARNING 123 | #define LOG_2(format, ...) fprintf(LOG_OUT, format, ##__VA_ARGS__) 124 | #else 125 | #define LOG_2(format, ...) ((void)0) 126 | #endif 127 | #if LOG_LEVEL >= LOG_MSG 128 | #define LOG_3(format, ...) fprintf(LOG_OUT, format, ##__VA_ARGS__) 129 | #else 130 | #define LOG_3(format, ...) ((void)0) 131 | #endif 132 | #define LOG_4(format, ...) ((void)0) 133 | #define LOG_(level, format, ...) LOG_##level (format, ##__VA_ARGS__) 134 | #define LOG(level, format, ...) LOG_(level, format"\n", ##__VA_ARGS__) 135 | 136 | #define LOGA(format, ...) LOG(LOG_NONE, format, ##__VA_ARGS__) 137 | #define LOGA_INFO(format, ...) LOG(LOG_NONE, "<%s:%d>: " format, ##__VA_ARGS__) 138 | #define LOGE(format, ...) LOG(LOG_ERROR, STR_CTR(LOG_ERROR_COLOR, ": " format), ## __VA_ARGS__) 139 | #define LOGW(format, ...) LOG(LOG_WARNING, STR_CTR(LOG_WARNING_COLOR,": " format), ## __VA_ARGS__) 140 | #define LOGM(format, ...) LOG(LOG_MSG, STR_CTR(LOG_MSG_COLOR, ": " format), ## __VA_ARGS__) 141 | #define LOGE_INFO(format, ...) LOG(LOG_ERROR, \ 142 | STR_CTR(LOG_ERROR_COLOR, "<") \ 143 | STR_CTR(LOG_LINK_COLOR, "%s:%d") \ 144 | STR_CTR(LOG_ERROR_COLOR, " ERROR>: " format), \ 145 | __FILE__, __LINE__, ##__VA_ARGS__) 146 | #define LOGW_INFO(format, ...) LOG(LOG_WARNING, \ 147 | STR_CTR(LOG_WARNING_COLOR,"<") \ 148 | STR_CTR(LOG_LINK_COLOR,"%s:%d") \ 149 | STR_CTR(LOG_WARNING_COLOR, " WARNING>: " format), \ 150 | __FILE__, __LINE__, ##__VA_ARGS__) 151 | #define LOGM_INFO(format, ...) LOG(LOG_MSG, \ 152 | STR_CTR(LOG_MSG_COLOR, "<") \ 153 | STR_CTR(LOG_LINK_COLOR, "%s:%d") \ 154 | STR_CTR(LOG_MSG_COLOR, " MSG>: " format), \ 155 | __FILE__, __LINE__, ##__VA_ARGS__) 156 | 157 | /******************** the time counter API ************************/ 158 | #if !defined(DO_NOT_CNT_TIME) && LOG_LEVEL > LOG_NONE 159 | #define CNT_TIME(tag, codes, ...) do{ \ 160 | class { \ 161 | private: \ 162 | systime begin; \ 163 | public: \ 164 | TimeCounter(){ \ 165 | getsystime(begin); \ 166 | } \ 167 | ~TimeCounter(){ \ 168 | systime end; \ 169 | getsystime(end); \ 170 | LOGM(tag": %.1lfms", ##__VA_ARGS__, getTimeIntervalms(end, begin)); \ 171 | } \ 172 | } time_cnt; \ 173 | codes; \ 174 | }while (0) 175 | #else 176 | #define CNT_TIME(tag, codes, ...) codes 177 | #endif 178 | #else /* _LOG_H_ */ 179 | #warning "Multiple include of log.h, some settings may not work." 180 | #endif /* _LOG_H_ */ 181 | -------------------------------------------------------------------------------- /others/src/camera/camera_wrapper.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zhikun on 18-11-7. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace cv; 13 | 14 | CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std::string &n) : 15 | name(n), 16 | init_done(false), 17 | mode(camera_mode), 18 | camera_cnts(2), 19 | camera_status(-1), 20 | iplImage(nullptr), 21 | rgb_buffer(nullptr), 22 | channel(3), 23 | gain(gain), 24 | exposure(exposure){ 25 | } 26 | 27 | void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext){ 28 | CameraWrapper *c = (CameraWrapper*)pContext; 29 | CameraImageProcess(hCamera, pFrameBuffer, c->rgb_buffer, pFrameHead); 30 | auto iplImage = cvCreateImageHeader(cvSize(pFrameHead->iWidth, pFrameHead->iHeight), IPL_DEPTH_8U, c->channel); 31 | cvSetData(iplImage, c->rgb_buffer, pFrameHead->iWidth * c->channel); //此处只是设置指针，无图像块数据拷贝，不需担心转换效率 32 | c->src_queue.push(cv::cvarrToMat(iplImage).clone()); 33 | } 34 | 35 | bool CameraWrapper::init() { 36 | CameraSdkInit(1); 37 | int camera_enumerate_device_status = CameraEnumerateDevice(camera_enum_list, &camera_cnts); 38 | if (camera_enumerate_device_status != CAMERA_STATUS_SUCCESS) { 39 | LOGE("CameraEnumerateDevice fail with %d!", camera_enumerate_device_status); 40 | } 41 | if (camera_cnts == 0) { 42 | LOGE("No camera device detected!"); 43 | return false; 44 | } else if (camera_cnts >= 1) { 45 | LOGM("%d camera device detected!", camera_cnts); 46 | } 47 | int i; 48 | for (i = 0; i < camera_cnts; i++) { 49 | camera_status = CameraInit(&camera_enum_list[i], -1, -1, &h_camera); 50 | if (camera_status != CAMERA_STATUS_SUCCESS) { 51 | CameraUnInit(h_camera); 52 | continue; 53 | } 54 | CameraGetFriendlyName(h_camera, camera_name); 55 | if (name == "NULL" || strcmp(name.data(), camera_name) == 0) { 56 | break; 57 | } 58 | CameraUnInit(h_camera); 59 | } 60 | if (i >= camera_cnts) { 61 | LOGE("No device name %s or device open error!!", name.data()); 62 | return false; 63 | } 64 | 65 | auto status = CameraGetCapability(h_camera, &tCapability); 66 | if (status != CAMERA_STATUS_SUCCESS) { 67 | cout << "CameraGetCapability return error code " << status << endl; 68 | return false; 69 | } 70 | 71 | rgb_buffer = (unsigned char *) malloc(tCapability.sResolutionRange.iHeightMax * 72 | tCapability.sResolutionRange.iWidthMax * 3); 73 | #ifdef Windows 74 | char filepath[200]; 75 | sprintf(filepath, PROJECT_DIR"/others/%s.Config", name.data()); 76 | if (CameraReadParameterFromFile(h_camera, filepath) != CAMERA_STATUS_SUCCESS) { 77 | LOGE("Load parameter %s from file fail!", filepath); 78 | return false; 79 | } 80 | if (CameraLoadParameter(h_camera, PARAMETER_TEAM_A) != CAMERA_STATUS_SUCCESS) { 81 | LOGE("CameraLoadParameter %s fail!", filepath); 82 | return false; 83 | } 84 | LOGM("successfully loaded %s!", filepath); 85 | #elif defined(Linux) 86 | CameraReadParameterFromFile(h_camera, PROJECT_DIR"/others/MV-UB31-Group0.config"); 87 | CameraLoadParameter(h_camera, PARAMETER_TEAM_A); 88 | CameraSetAeState(h_camera, false); 89 | CameraSetExposureTime(h_camera, exposure * 1000); 90 | CameraSetAnalogGain(h_camera, gain); 91 | #endif 92 | double t; 93 | int g; 94 | CameraGetExposureTime(h_camera, &t); 95 | CameraGetAnalogGain(h_camera, &g); 96 | LOGM("Exposure time: %lfms, gain:%d", t / 1000.0, g); 97 | /*让SDK进入工作模式，开始接收来自相机发送的图像 98 | 数据。如果当前相机是触发模式，则需要接收到 99 | 触发帧以后才会更新图像。 */ 100 | CameraPlay(h_camera); 101 | 102 | /*其他的相机参数设置 103 | 例如 CameraSetExposureTime CameraGetExposureTime 设置/读取曝光时间 104 | CameraSetImageResolution CameraGetImageResolution 设置/读取分辨率 105 | CameraSetGamma、CameraSetContrast、CameraSetGain等设置图像伽马、对比度、RGB数字增益等等。 106 | CameraGetFriendlyName CameraSetFriendlyName 获取/设置相机名称（该名称可写入相机硬件） 107 | */ 108 | cout << tCapability.sIspCapacity.bMonoSensor << endl; 109 | if (tCapability.sIspCapacity.bMonoSensor) { 110 | channel = 1; 111 | CameraSetIspOutFormat(h_camera, CAMERA_MEDIA_TYPE_MONO8); 112 | LOGM("camera %s mono ", camera_name); 113 | } else { 114 | channel = 3; 115 | CameraSetIspOutFormat(h_camera, CAMERA_MEDIA_TYPE_BGR8); 116 | LOGM("camera %s color ", camera_name); 117 | } 118 | if(mode == 2){ 119 | CameraSetCallbackFunction(h_camera, cameraCallback, this, nullptr); 120 | } 121 | init_done = true; 122 | return true; 123 | } 124 | 125 | bool CameraWrapper::read(cv::Mat &src) { 126 | if(init_done) { 127 | if (mode == 0)return readProcessed(src); 128 | if (mode == 1)return readRaw(src); 129 | if (mode == 2)return readCallback(src); 130 | } else { 131 | return false; 132 | } 133 | } 134 | 135 | bool CameraWrapper::readRaw(cv::Mat &src) { 136 | if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) { 137 | if (iplImage) { 138 | cvReleaseImageHeader(&iplImage); 139 | } 140 | 141 | iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, 1); 142 | 143 | cvSetData(iplImage, pby_buffer, frame_info.iWidth); //此处只是设置指针，无图像块数据拷贝，不需担心转换效率 144 | 145 | src = cv::cvarrToMat(iplImage).clone(); 146 | 147 | //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。 148 | //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer 149 | CameraReleaseImageBuffer(h_camera, pby_buffer); 150 | 151 | return true; 152 | } else { 153 | src = cv::Mat(); 154 | return false; 155 | } 156 | } 157 | 158 | bool CameraWrapper::readProcessed(cv::Mat &src) { 159 | // cerr << "Get-1" << endl; 160 | if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) { 161 | CameraImageProcess(h_camera, pby_buffer, rgb_buffer, 162 | &frame_info); // this function is super slow, better not to use it. 163 | if (iplImage) { 164 | cvReleaseImageHeader(&iplImage); 165 | } 166 | iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, channel); 167 | cvSetData(iplImage, rgb_buffer, frame_info.iWidth * channel); //此处只是设置指针，无图像块数据拷贝，不需担心转换效率 168 | src = cv::cvarrToMat(iplImage).clone(); 169 | //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。 170 | //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer 171 | CameraReleaseImageBuffer(h_camera, pby_buffer); 172 | return true; 173 | } else { 174 | src = cv::Mat(); 175 | return false; 176 | } 177 | } 178 | 179 | bool CameraWrapper::readCallback(cv::Mat &src) { 180 | systime ts, te; 181 | getsystime(ts); 182 | while(src_queue.empty()){ 183 | getsystime(te); 184 | if(getTimeIntervalms(te, ts) > 500){ 185 | return false; 186 | } 187 | } 188 | return src_queue.pop(src); 189 | } 190 | 191 | CameraWrapper::~CameraWrapper() { 192 | CameraUnInit(h_camera); 193 | //注意，先反初始化后再free 194 | if (rgb_buffer != nullptr) 195 | free(rgb_buffer); 196 | } 197 | -------------------------------------------------------------------------------- /armor/src/armor_finder/find/find_armor_box.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xinyang on 19-7-18. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define DO_NOT_CNT_TIME 11 | 12 | #include 13 | 14 | // 判断两个灯条的角度差 15 | static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 16 | float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle : 17 | light_blob_i.rect.angle - 90; 18 | float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : 19 | light_blob_j.rect.angle - 90; 20 | return abs(angle_i - angle_j) < 20; 21 | } 22 | // 判断两个灯条的高度差 23 | static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 24 | cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; 25 | return abs(centers.y) < 30; 26 | } 27 | // 判断两个灯条的间距 28 | static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 29 | double side_length; 30 | cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; 31 | side_length = sqrt(centers.ddot(centers)); 32 | return (side_length / light_blob_i.length < 10 && side_length / light_blob_i.length > 0.5); 33 | } 34 | // 判断两个灯条的长度比 35 | static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 36 | return (light_blob_i.length / light_blob_j.length < 2.5 37 | && light_blob_i.length / light_blob_j.length > 0.4); 38 | } 39 | 40 | /* 判断两个灯条的错位度，不知道英文是什么！！！ */ 41 | static bool CuoWeiDuJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 42 | float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle : 43 | light_blob_i.rect.angle - 90; 44 | float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : 45 | light_blob_j.rect.angle - 90; 46 | float angle = (angle_i + angle_j) / 2.0 / 180.0 * 3.14159265459; 47 | if (abs(angle_i - angle_j) > 90) { 48 | angle += 3.14159265459 / 2; 49 | } 50 | Vector2f orientation(cos(angle), sin(angle)); 51 | Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x, 52 | light_blob_j.rect.center.y - light_blob_i.rect.center.y); 53 | return abs(orientation.dot(p2p)) < 25; 54 | } 55 | // 判断装甲板方向 56 | static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { 57 | float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle : 58 | light_blob_i.rect.angle - 90; 59 | float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : 60 | light_blob_j.rect.angle - 90; 61 | float angle = (angle_i + angle_j) / 2.0; 62 | if (abs(angle_i - angle_j) > 90) { 63 | angle += 90.0; 64 | } 65 | return (-120.0 < angle && angle < -60.0) || (60.0 < angle && angle < 120.0); 66 | } 67 | // 判断两个灯条是否可以匹配 68 | static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) { 69 | return light_blob_i.blob_color == enemy_color && 70 | light_blob_j.blob_color == enemy_color && 71 | lengthRatioJudge(light_blob_i, light_blob_j) && 72 | lengthJudge(light_blob_i, light_blob_j) && 73 | // heightJudge(light_blob_i, light_blob_j) && 74 | angelJudge(light_blob_i, light_blob_j) && 75 | boxAngleJudge(light_blob_i, light_blob_j) && 76 | CuoWeiDuJudge(light_blob_i, light_blob_j); 77 | 78 | } 79 | // 匹配所有灯条，得出装甲板候选区 80 | bool ArmorFinder::matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes) { 81 | armor_boxes.clear(); 82 | for (int i = 0; i < light_blobs.size() - 1; ++i) { 83 | for (int j = i + 1; j < light_blobs.size(); ++j) { 84 | if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), enemy_color)) { 85 | continue; 86 | } 87 | cv::Rect2d rect_left = light_blobs.at(static_cast(i)).rect.boundingRect(); 88 | cv::Rect2d rect_right = light_blobs.at(static_cast(j)).rect.boundingRect(); 89 | double min_x, min_y, max_x, max_y; 90 | min_x = fmin(rect_left.x, rect_right.x) - 4; 91 | max_x = fmax(rect_left.x + rect_left.width, rect_right.x + rect_right.width) + 4; 92 | min_y = fmin(rect_left.y, rect_right.y) - 0.5 * (rect_left.height + rect_right.height) / 2.0; 93 | max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + 94 | 0.5 * (rect_left.height + rect_right.height) / 2.0; 95 | if (min_x < 0 || max_x > src.cols || min_y < 0 || max_y > src.rows) { 96 | continue; 97 | } 98 | if (state == SEARCHING_STATE && (max_y + min_y) / 2 < 120) continue; 99 | if ((max_x - min_x) / (max_y - min_y) < 0.8) continue; 100 | LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)}; 101 | armor_boxes.emplace_back( 102 | cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y), 103 | pair_blobs, 104 | enemy_color 105 | ); 106 | } 107 | } 108 | return !armor_boxes.empty(); 109 | } 110 | // 在给定的图像上寻找装甲板 111 | bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) { 112 | LightBlobs light_blobs; // 存储所有可能的灯条 113 | ArmorBoxes armor_boxes; // 装甲板候选区 114 | 115 | box.rect = cv::Rect2d(0, 0, 0, 0); 116 | box.id = -1; 117 | // 寻找所有可能的灯条 118 | CNT_TIME("blob", { 119 | if (!findLightBlobs(src, light_blobs)) { 120 | return false; 121 | } 122 | }); 123 | if (show_light_blobs && state==SEARCHING_STATE) { 124 | showLightBlobs("light_blobs", src, light_blobs); 125 | cv::waitKey(1); 126 | } 127 | // 对灯条进行匹配得出装甲板候选区 128 | CNT_TIME("boxes", { 129 | if (!matchArmorBoxes(src, light_blobs, armor_boxes)) { 130 | return false; 131 | } 132 | }); 133 | if (show_armor_boxes && state==SEARCHING_STATE) { 134 | showArmorBoxes("boxes", src, armor_boxes); 135 | cv::waitKey(1); 136 | } 137 | // 如果分类器可用，则使用分类器对装甲板候选区进行筛选 138 | if (classifier) { 139 | CNT_TIME("classify: %d", { 140 | for (auto &armor_box : armor_boxes) { 141 | cv::Mat roi = src(armor_box.rect).clone(); 142 | cv::resize(roi, roi, cv::Size(48, 36)); 143 | int c = classifier(roi); 144 | armor_box.id = c; 145 | } 146 | }, armor_boxes.size()); 147 | // 按照优先级对装甲板进行排序 148 | sort(armor_boxes.begin(), armor_boxes.end(), [&](const ArmorBox &a, const ArmorBox &b) { 149 | if (last_box.rect != cv::Rect2d()) { 150 | return getPointLength(a.getCenter() - last_box.getCenter()) < 151 | getPointLength(b.getCenter() - last_box.getCenter()); 152 | } else { 153 | return a < b; 154 | } 155 | }); 156 | for (auto &one_box : armor_boxes) { 157 | if (one_box.id != 0) { 158 | box = one_box; 159 | break; 160 | } 161 | } 162 | if (save_labelled_boxes) { 163 | for (const auto &one_box : armor_boxes) { 164 | char filename[100]; 165 | sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[one_box.id].data(), 166 | time(nullptr) + clock()); 167 | auto box_roi = src(one_box.rect); 168 | cv::resize(box_roi, box_roi, cv::Size(48, 36)); 169 | cv::imwrite(filename, box_roi); 170 | } 171 | } 172 | if (box.rect == cv::Rect2d(0, 0, 0, 0)) { 173 | return false; 174 | } 175 | if (show_armor_boxes && state==SEARCHING_STATE) { 176 | showArmorBoxesClass("class", src, armor_boxes); 177 | } 178 | } else { // 如果分类器不可用，则直接选取候选区中的第一个区域作为目标(往往会误识别) 179 | box = armor_boxes[0]; 180 | } 181 | return true; 182 | } 183 | 184 | 185 | -------------------------------------------------------------------------------- /tools/para/conv2_w: -------------------------------------------------------------------------------- 1 | 6 2 | 10 3 | 3 4 | 3 5 | -0.18567735 6 | 0.31230295 7 | 0.10601954 8 | -0.14635935 9 | 0.17529164 10 | 0.23833705 11 | -0.07737787 12 | -0.26470512 13 | -0.016763074 14 | 0.0732159 15 | 0.15613954 16 | 0.24339953 17 | 0.21809842 18 | 0.10773516 19 | 0.40015697 20 | 0.08531675 21 | 0.25792578 22 | 0.29302046 23 | -0.14973466 24 | -0.13794622 25 | -0.3174705 26 | 0.06396363 27 | -0.1582382 28 | -0.2843077 29 | -0.25538033 30 | -0.23979947 31 | -0.2003432 32 | -0.067921035 33 | 0.008744139 34 | -0.12954548 35 | 0.07350555 36 | 0.016850626 37 | -0.13139062 38 | 0.07412223 39 | -0.07210239 40 | -0.042953018 41 | -0.3169445 42 | 0.009288535 43 | 0.30465534 44 | -0.4208047 45 | 0.07115591 46 | 0.26925838 47 | -0.38791 48 | 0.06488703 49 | 0.400365 50 | -0.5090438 51 | -0.05041841 52 | 0.28737134 53 | -0.32649037 54 | 0.2732965 55 | 0.3234882 56 | -0.14768949 57 | 0.19571733 58 | 0.46035185 59 | -0.1308439 60 | -0.28924602 61 | -0.4150856 62 | -0.1394865 63 | -0.14277102 64 | -0.028665107 65 | 0.063959725 66 | -0.08086398 67 | -0.42271927 68 | -1.0998808 69 | -1.7531674 70 | -0.6868419 71 | -0.4679067 72 | -1.1683141 73 | -0.3862623 74 | 0.1865768 75 | -0.7758855 76 | 0.18598925 77 | -0.2777104 78 | -0.5689085 79 | -0.53544736 80 | 0.06992068 81 | 0.09683173 82 | -0.05572889 83 | 0.497631 84 | 0.5235087 85 | 0.412554 86 | 0.35491413 87 | 0.20174716 88 | 0.14414282 89 | 0.21999303 90 | 0.14763339 91 | 0.15525024 92 | 0.13345838 93 | -0.07630705 94 | -0.007321237 95 | -0.17791471 96 | 0.067984045 97 | 0.07404319 98 | -0.2620913 99 | -0.020879569 100 | -0.09856246 101 | -0.27126157 102 | -0.19668914 103 | -0.45068544 104 | 0.06535956 105 | 0.19665721 106 | 0.4605025 107 | 0.0834005 108 | 0.26365396 109 | 0.42590958 110 | 0.041509036 111 | 0.28209603 112 | 0.23930955 113 | -0.16162594 114 | -0.22635049 115 | -0.26243812 116 | -0.12631208 117 | -0.22097321 118 | -0.20312756 119 | -0.1772115 120 | -0.33603805 121 | -0.30561772 122 | 0.008207564 123 | 0.07136626 124 | -0.216243 125 | 0.16515563 126 | 0.04111277 127 | -0.1260301 128 | 0.14099488 129 | 0.10370603 130 | -0.010030857 131 | -0.27448004 132 | -0.011123834 133 | 0.22102898 134 | -0.25228068 135 | 0.039984487 136 | 0.24022454 137 | -0.12004822 138 | 0.10729556 139 | 0.22277792 140 | -0.2681527 141 | 0.1929927 142 | 0.35833898 143 | -0.1897413 144 | 0.2386225 145 | 0.59820104 146 | -0.27273512 147 | 0.1098645 148 | 0.36686987 149 | -0.43986773 150 | -0.18380699 151 | -0.43966547 152 | -0.009675484 153 | -0.25631252 154 | -0.29056635 155 | 0.0712151 156 | -0.18025662 157 | -0.48891056 158 | -0.47694656 159 | -0.88816905 160 | -0.06483537 161 | 0.013368553 162 | -0.70404077 163 | 0.28231522 164 | 0.33836752 165 | -0.22945245 166 | 0.25680065 167 | -0.0012361507 168 | -0.52792823 169 | -0.2579657 170 | 0.3255992 171 | -0.0019560119 172 | -0.046853587 173 | 0.24827203 174 | 0.34998816 175 | 0.27723187 176 | 0.31715482 177 | 0.34362832 178 | 0.12269884 179 | 0.13557787 180 | -0.114973396 181 | 0.07401403 182 | 0.069110855 183 | -0.187023 184 | -0.036038406 185 | 0.012674593 186 | 0.36098468 187 | 0.42171153 188 | 0.048450485 189 | 0.28105047 190 | 0.4715788 191 | -0.023357779 192 | -0.057762705 193 | -0.073359944 194 | -0.16736583 195 | 0.12476493 196 | 0.0062710703 197 | -0.021589287 198 | -0.13475952 199 | 0.030259239 200 | -0.036233522 201 | 0.06367138 202 | -0.07908364 203 | 0.36171123 204 | 0.056995805 205 | 0.26451215 206 | 0.18291938 207 | 0.06195444 208 | 0.23869722 209 | 0.10497623 210 | 0.09330571 211 | 0.29085782 212 | -0.2596862 213 | -0.031500176 214 | -0.013901459 215 | 0.023374349 216 | 0.016828336 217 | 0.11964698 218 | 0.026195215 219 | 0.035629973 220 | 0.12736408 221 | -0.42047524 222 | -0.045428358 223 | 0.4764139 224 | -0.594475 225 | -0.14393432 226 | 0.3904298 227 | -0.18805574 228 | -0.08113497 229 | 0.5267287 230 | -0.4067696 231 | -0.32341024 232 | -0.29679683 233 | -0.10745416 234 | 0.08389849 235 | 0.14507549 236 | -0.024750415 237 | 0.4222244 238 | 0.16702761 239 | -0.008214667 240 | 0.03591805 241 | 0.08528825 242 | 0.12195521 243 | 0.41446808 244 | 0.46344396 245 | 0.37176162 246 | 0.40626523 247 | 0.08781661 248 | -0.8886079 249 | -1.0024366 250 | -0.86886346 251 | -0.49839306 252 | -0.8621685 253 | -0.642396 254 | -0.13097173 255 | -0.7362578 256 | -0.22594142 257 | -0.07626368 258 | -0.5901063 259 | -0.3290703 260 | 0.14976943 261 | -0.06745293 262 | -0.0820076 263 | 0.5327264 264 | 0.4384248 265 | 0.6160922 266 | 0.24003918 267 | 0.24436 268 | 0.22210835 269 | 0.21770608 270 | 0.124687046 271 | 0.0470902 272 | -0.14859878 273 | -0.321342 274 | -0.08767322 275 | 0.20693085 276 | 0.42665282 277 | 0.33726025 278 | -0.020886194 279 | 0.44385502 280 | 0.01808433 281 | -0.118666425 282 | -0.15688846 283 | -0.31219846 284 | 0.20918387 285 | 0.021502182 286 | 0.33103526 287 | 0.24867874 288 | 0.21629289 289 | 0.013217075 290 | 0.08715608 291 | 0.054708257 292 | -0.15127313 293 | 0.1508026 294 | 0.24620876 295 | 0.44316766 296 | 0.17231683 297 | 0.1236346 298 | 0.26164678 299 | -0.08726074 300 | -0.052426733 301 | 0.33535057 302 | 0.36311948 303 | 0.41015378 304 | 0.34961388 305 | 0.09574385 306 | 0.5627171 307 | 0.4841079 308 | 0.36436325 309 | 0.62349635 310 | 0.651723 311 | -0.4531835 312 | 0.15029465 313 | 0.57511115 314 | -0.45798486 315 | 0.19631258 316 | 0.3225775 317 | -0.103153236 318 | 0.19025934 319 | 0.48245525 320 | -0.09091705 321 | -0.094119325 322 | -0.38689196 323 | 0.14930637 324 | 0.09523474 325 | -0.04373269 326 | 0.3126633 327 | 0.08655161 328 | -0.082390875 329 | -0.053303 330 | 0.31841794 331 | 0.3719926 332 | 0.15074554 333 | 0.40985286 334 | 0.33657342 335 | 0.20797513 336 | 0.24987732 337 | 0.39347094 338 | 0.83306986 339 | 0.86048186 340 | 0.5689052 341 | 0.72829825 342 | 0.89263994 343 | 0.8195005 344 | 0.61129767 345 | 0.78122336 346 | 0.61171514 347 | -0.319987 348 | -0.25968623 349 | -0.11498821 350 | 0.23861922 351 | 0.088788316 352 | 0.2950782 353 | 0.62382466 354 | 0.8079981 355 | 0.61313707 356 | 0.077279635 357 | -0.032932185 358 | 0.0890238 359 | 0.029694919 360 | 0.12197181 361 | -0.042123534 362 | -0.29403502 363 | -0.36851963 364 | 0.09636119 365 | 0.04346845 366 | 0.43912384 367 | 0.3536572 368 | 0.066943154 369 | 0.4024961 370 | 0.47593385 371 | 0.0759621 372 | 0.39561898 373 | 0.22179781 374 | -0.35304305 375 | -0.20236854 376 | -0.008968132 377 | -0.09802803 378 | 0.054423235 379 | 0.23025425 380 | -0.015376655 381 | 0.008936614 382 | 0.2355261 383 | 0.0014180358 384 | 0.17833155 385 | 0.19288565 386 | 0.28466016 387 | -0.02708328 388 | 0.05748906 389 | 0.115661204 390 | -0.08906293 391 | 0.06937998 392 | -0.12015559 393 | -0.11914039 394 | -0.15230455 395 | -0.30971813 396 | 0.12253699 397 | -0.1287006 398 | -0.11642937 399 | -0.03524333 400 | 0.106573164 401 | -0.46892896 402 | 0.048909124 403 | 0.35770398 404 | -0.48432148 405 | 0.06230857 406 | 0.35268116 407 | -0.6384595 408 | 0.054048087 409 | 0.33101568 410 | -0.7891181 411 | -0.5678995 412 | -0.32257825 413 | -0.30838996 414 | -0.019490078 415 | 0.20418139 416 | 0.2235779 417 | 0.43282986 418 | 0.33755836 419 | -0.43536973 420 | -0.18715984 421 | -0.017235223 422 | -0.22174615 423 | 0.009737464 424 | 0.18385953 425 | 0.3702021 426 | 0.1722581 427 | 0.31182298 428 | -1.5388889 429 | -1.4710528 430 | -0.9808655 431 | -1.2328979 432 | -1.391655 433 | -0.9775159 434 | -0.53418314 435 | -1.0619466 436 | -0.14114031 437 | -0.38471037 438 | -0.91709787 439 | -0.6730362 440 | -0.3246664 441 | -0.50130564 442 | -0.29241592 443 | 0.2488604 444 | 0.29896885 445 | 0.42214942 446 | 0.23901664 447 | 0.071325876 448 | 0.0730879 449 | 0.3215266 450 | 0.37580463 451 | 0.17601205 452 | 0.082663685 453 | -0.14512439 454 | -0.02447424 455 | 0.036292106 456 | 0.190385 457 | 0.24941531 458 | -0.15101732 459 | 0.16009882 460 | 0.022328991 461 | -0.13642876 462 | -0.2557106 463 | -0.23009326 464 | -0.2648493 465 | 0.035393387 466 | 0.058671933 467 | -0.06313441 468 | 0.085322335 469 | 0.037474677 470 | -0.09684385 471 | 0.021198915 472 | -0.23987578 473 | 0.49218395 474 | 0.45345736 475 | 0.4600616 476 | 0.27070585 477 | 0.23068464 478 | 0.37693858 479 | 0.32365304 480 | 0.2629329 481 | 0.38881606 482 | 0.25656843 483 | 0.3796918 484 | 0.45741585 485 | 0.08370856 486 | 0.29546905 487 | 0.55899864 488 | 0.16390188 489 | 0.43054807 490 | 0.6232953 491 | -0.6125586 492 | 0.042835608 493 | 0.42402682 494 | -0.4055307 495 | -0.039988685 496 | 0.42992234 497 | -0.26697057 498 | 0.10284304 499 | 0.4822276 500 | -0.32137555 501 | -0.38547745 502 | -0.33756226 503 | 0.12396629 504 | 0.018000755 505 | 0.0040020915 506 | 0.09871851 507 | 0.19964874 508 | -0.049439877 509 | -0.054510776 510 | 0.20413962 511 | 0.49790758 512 | 0.3678443 513 | 0.47925648 514 | 0.5145687 515 | 0.46790117 516 | 0.4268048 517 | 0.23691703 518 | 0.7796671 519 | 0.6537224 520 | 0.6620804 521 | 0.6199029 522 | 0.6739477 523 | 0.5245343 524 | 0.7053961 525 | 0.5178989 526 | 0.678157 527 | 0.015971098 528 | -0.09368621 529 | -0.1418303 530 | 0.2623539 531 | 0.27078906 532 | 0.26214978 533 | 0.450224 534 | 0.5519793 535 | 0.589996 536 | 0.0695408 537 | 0.11883035 538 | 0.16186124 539 | -0.00019518172 540 | -0.10749374 541 | -0.07061164 542 | -0.37467608 543 | -0.33476043 544 | -0.19680664 545 | -------------------------------------------------------------------------------- /tools/TrainCNN/backward.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | print("Preparing...") 3 | import tensorflow as tf 4 | import os 5 | from tqdm import tqdm 6 | import generate 7 | import forward 8 | import cv2 9 | import numpy as np 10 | import mvsdk 11 | 12 | print("Finish!") 13 | 14 | 15 | def save_kernal(fp, val): 16 | print(val.shape[2], file=fp) 17 | print(val.shape[3], file=fp) 18 | print(val.shape[1], file=fp) 19 | print(val.shape[0], file=fp) 20 | for in_channel in range(val.shape[2]): 21 | for out_channel in range(val.shape[3]): 22 | for row in range(val.shape[0]): 23 | for col in range(val.shape[1]): 24 | print(val[row][col][in_channel][out_channel], file=fp) 25 | 26 | 27 | def save_weight_mat(fp, val): 28 | print(val.shape[0], file=fp) 29 | print(val.shape[1], file=fp) 30 | for row in range(val.shape[0]): 31 | for col in range(val.shape[1]): 32 | print(val[row][col], file=fp) 33 | 34 | 35 | def save_bias(fp, val): 36 | print(val.shape[0], file=fp) 37 | for i in range(val.shape[0]): 38 | print(val[i], file=fp) 39 | 40 | 41 | def save_para(folder, paras, names, info): 42 | os.system("mkdir %s/%s" % (folder, info)) 43 | for para, name in zip(paras, names): 44 | fp = open("%s/%s/%s" % (folder, info, name), "w") 45 | if name[-1:] == "b": 46 | save_bias(fp, para) 47 | elif name[:2] == "fc": 48 | save_weight_mat(fp, para) 49 | elif name[:4] == "conv": 50 | save_kernal(fp, para) 51 | fp.close() 52 | 53 | 54 | STEPS = 100000 55 | BATCH = 40 56 | LEARNING_RATE_BASE = 0.0003 57 | LEARNING_RATE_DECAY = 0.99 58 | MOVING_AVERAGE_DECAY = 0.99 59 | 60 | 61 | def train(dataset, show_bar=False): 62 | x = tf.placeholder(tf.float32, [None, generate.SRC_ROWS, generate.SRC_COLS, generate.SRC_CHANNELS]) 63 | y_ = tf.placeholder(tf.float32, [None, forward.OUTPUT_NODES]) 64 | keep_rate = tf.placeholder(tf.float32) 65 | nodes, vars, vars_name = forward.forward(x, 0.01, keep_rate) 66 | y = nodes[-1] 67 | 68 | ce = tf.nn.sparse_softmax_cross_entropy_with_logits(logits=y, labels=tf.argmax(y_, 1)) 69 | # ce = tf.nn.weighted_cross_entropy_with_logits(logits=y, labels=tf.argmax(y_, 1), pos_weight=1) 70 | cem = tf.reduce_mean(ce) 71 | loss = cem + tf.add_n(tf.get_collection("losses")) 72 | 73 | global_step = tf.Variable(0, trainable=False) 74 | learning_rate = tf.train.exponential_decay( 75 | LEARNING_RATE_BASE, 76 | global_step, 77 | len(dataset.train_samples) / BATCH, 78 | LEARNING_RATE_DECAY, 79 | staircase=False) 80 | train_step = tf.train.AdamOptimizer(learning_rate).minimize(loss, global_step=global_step) 81 | 82 | ema = tf.train.ExponentialMovingAverage(MOVING_AVERAGE_DECAY, global_step) 83 | ema_op = ema.apply(tf.trainable_variables()) 84 | with tf.control_dependencies([train_step, ema_op]): 85 | train_op = tf.no_op(name='train') 86 | 87 | correct_prediction = tf.equal(tf.argmax(y, 1), tf.argmax(y_, 1)) 88 | accuracy = tf.reduce_mean(tf.cast(correct_prediction, tf.float32)) 89 | 90 | config = tf.ConfigProto(gpu_options=tf.GPUOptions(allow_growth=True)) 91 | with tf.Session(config=config) as sess: 92 | init_op = tf.global_variables_initializer() 93 | sess.run(init_op) 94 | 95 | bar = tqdm(range(STEPS), ascii=True, dynamic_ncols=True) 96 | for i in bar: 97 | images_samples, labels_samples = dataset.sample_train_sets(BATCH, 0.03) 98 | 99 | _, loss_value, step = sess.run( 100 | [train_op, loss, global_step], 101 | feed_dict={x: images_samples, y_: labels_samples, keep_rate: 0.3} 102 | ) 103 | 104 | if step % 500 == 0: 105 | test_images, test_labels = dataset.sample_test_sets(10000) 106 | test_acc, output = sess.run([accuracy, y], 107 | feed_dict={x: test_images, y_: test_labels, keep_rate: 1.0}) 108 | output = np.argmax(output, axis=1) 109 | real = np.argmax(test_labels, axis=1) 110 | print("=============test-set===============") 111 | for n in range(forward.OUTPUT_NODES): 112 | print("label: %d, precise: %f, recall: %f" % 113 | (n, np.mean(real[output == n] == n), np.mean(output[real == n] == n))) 114 | 115 | train_images, train_labels = dataset.sample_train_sets(10000) 116 | train_acc, output = sess.run([accuracy, y], 117 | feed_dict={x: train_images, y_: train_labels, keep_rate: 1.0}) 118 | output = np.argmax(output, axis=1) 119 | real = np.argmax(train_labels, axis=1) 120 | print("=============train-set===============") 121 | for n in range(forward.OUTPUT_NODES): 122 | print("label: %d, precise: %f, recall: %f" % 123 | (n, np.mean(real[output == n] == n), np.mean(output[real == n] == n))) 124 | print("\n") 125 | if train_acc >= 0.99 and test_acc >= 0.99: 126 | vars_val = sess.run(vars) 127 | save_para( 128 | "model", 129 | vars_val, 130 | vars_name, 131 | "steps:%d-train_acc:%f-test_acc:%f" % (step, train_acc, test_acc) 132 | ) 133 | bar.set_postfix({"loss": loss_value, "train_acc": train_acc, "test_acc": test_acc}) 134 | # print("save done!") 135 | 136 | # pred = sess.run(y, feed_dict={x: test_images, keep_rate:1.0}) 137 | 138 | # nodes_val = sess.run(nodes, feed_dict={x:test_images}) 139 | # return vars_val, nodes_val 140 | DevList = mvsdk.CameraEnumerateDevice() 141 | nDev = len(DevList) 142 | if nDev < 1: 143 | print("No camera was found!") 144 | return 145 | 146 | for i, DevInfo in enumerate(DevList): 147 | print("{}: {} {}".format(i, DevInfo.GetFriendlyName(), DevInfo.GetPortType())) 148 | i = 0 if nDev == 1 else int(input("Select camera: ")) 149 | DevInfo = DevList[i] 150 | print(DevInfo) 151 | 152 | # 打开相机 153 | hCamera = 0 154 | try: 155 | hCamera = mvsdk.CameraInit(DevInfo, -1, -1) 156 | except mvsdk.CameraException as e: 157 | print("CameraInit Failed({}): {}".format(e.error_code, e.message)) 158 | return 159 | 160 | # 获取相机特性描述 161 | cap = mvsdk.CameraGetCapability(hCamera) 162 | 163 | # 判断是黑白相机还是彩色相机 164 | monoCamera = (cap.sIspCapacity.bMonoSensor != 0) 165 | 166 | # 黑白相机让ISP直接输出MONO数据，而不是扩展成R=G=B的24位灰度 167 | if monoCamera: 168 | mvsdk.CameraSetIspOutFormat(hCamera, mvsdk.CAMERA_MEDIA_TYPE_MONO8) 169 | else: 170 | mvsdk.CameraSetIspOutFormat(hCamera, mvsdk.CAMERA_MEDIA_TYPE_BGR8) 171 | 172 | # 相机模式切换成连续采集 173 | mvsdk.CameraSetTriggerMode(hCamera, 0) 174 | 175 | # 手动曝光，曝光时间30ms 176 | mvsdk.CameraSetAeState(hCamera, 0) 177 | mvsdk.CameraSetExposureTime(hCamera, 30 * 1000) 178 | 179 | # 让SDK内部取图线程开始工作 180 | mvsdk.CameraPlay(hCamera) 181 | 182 | # 计算RGB buffer所需的大小，这里直接按照相机的最大分辨率来分配 183 | FrameBufferSize = cap.sResolutionRange.iWidthMax * cap.sResolutionRange.iHeightMax * (1 if monoCamera else 3) 184 | 185 | # 分配RGB buffer，用来存放ISP输出的图像 186 | # 备注：从相机传输到PC端的是RAW数据，在PC端通过软件ISP转为RGB数据（如果是黑白相机就不需要转换格式，但是ISP还有其它处理，所以也需要分配这个buffer） 187 | pFrameBuffer = mvsdk.CameraAlignMalloc(FrameBufferSize, 16) 188 | 189 | while (cv2.waitKey(1) & 0xFF) != ord('q'): 190 | # 从相机取一帧图片 191 | try: 192 | pRawData, FrameHead = mvsdk.CameraGetImageBuffer(hCamera, 200) 193 | mvsdk.CameraImageProcess(hCamera, pRawData, pFrameBuffer, FrameHead) 194 | mvsdk.CameraReleaseImageBuffer(hCamera, pRawData) 195 | # 此时图片已经存储在pFrameBuffer中，对于彩色相机pFrameBuffer=RGB数据，黑白相机pFrameBuffer=8位灰度数据 196 | # 把pFrameBuffer转换成opencv的图像格式以进行后续算法处理 197 | frame_data = (mvsdk.c_ubyte * FrameHead.uBytes).from_address(pFrameBuffer) 198 | frame = np.frombuffer(frame_data, dtype=np.uint8) 199 | frame = frame.reshape((FrameHead.iHeight, FrameHead.iWidth, 200 | 1 if FrameHead.uiMediaType == mvsdk.CAMERA_MEDIA_TYPE_MONO8 else 3)) 201 | 202 | frame = cv2.resize(frame, (640, 480), interpolation=cv2.INTER_LINEAR) 203 | cv2.imshow("Press q to end", frame) 204 | if (cv2.waitKey(1) & 0xFF) == ord(' '): 205 | roi = cv2.selectROI("roi", frame) 206 | roi = frame[roi[1]:roi[1] + roi[3], roi[0]:roi[0] + roi[2]] 207 | print(roi) 208 | cv2.imshow("box", roi) 209 | image = cv2.resize(roi, (48, 36)) 210 | image = image.astype(np.float32) / 255.0 211 | out = sess.run(y, feed_dict={x: [image]}) 212 | print(out) 213 | print(np.argmax(out)) 214 | 215 | except mvsdk.CameraException as e: 216 | if e.error_code != mvsdk.CAMERA_STATUS_TIME_OUT: 217 | print("CameraGetImageBuffer failed({}): {}".format(e.error_code, e.message)) 218 | 219 | # 关闭相机 220 | mvsdk.CameraUnInit(hCamera) 221 | 222 | # 释放帧缓存 223 | mvsdk.CameraAlignFree(pFrameBuffer) 224 | 225 | 226 | if __name__ == "__main__": 227 | # import os 228 | # os.environ["CUDA_DEVICE_ORDER"] = "PCI_BUS_ID" 229 | # os.environ["CUDA_VISIBLE_DEVICES"] = "-1" 230 | dataset = generate.DataSet("/home/xinyang/Workspace/box_resize") 231 | train(dataset, show_bar=True) 232 | input("press enter to continue...") 233 | --------------------------------------------------------------------------------