diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp new file mode 100644 index 0000000..b1edf27 --- /dev/null +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -0,0 +1,68 @@ +// +// Created by xinyang on 19-7-15. +// + +#include +#include + +static double getTimeIntervalms(const timeval& now, const timeval &last){ + return (now.tv_sec-last.tv_sec)*1000.0 + (now.tv_usec-last.tv_usec)/1000.0; +} + +void ArmorFinder::antiTop() { + static double top_periodms = 0; + static double last1_top_periodms = 0; + static double last2_top_periodms = 0; + uint16_t shoot_delay = 0; + timeval curr_time; +// if(anti_top_state == ANTI_TOP){ +// cout << "anti top" << endl; +// }else if(anti_top_state == NORMAL){ +// cout << "Normal" << endl; +// } + gettimeofday(&curr_time, nullptr); + auto interval = getTimeIntervalms(curr_time, last_front_time); + if(interval > 700){ + anti_top_state = NORMAL; + anti_top_cnt = 0; + } + + ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); + if(orientation == ArmorBox::UNKNOWN){ + if(anti_top_state == NORMAL){ + sendBoxPosition(shoot_delay); + } + return; + } + + + if(orientation!=last_orient && orientation==ArmorBox::FRONT){ + last_front_time = curr_time; + if(150 4){ + anti_top_state = ANTI_TOP; + } + } + } + } + + if(anti_top_state == ANTI_TOP){ + if(orientation == ArmorBox::FRONT){ + sendBoxPosition(shoot_delay); + } + }else if(anti_top_state == NORMAL){ + sendBoxPosition(shoot_delay); + } + + last_orient = orientation; +} + diff --git a/armor/src/armor_finder/find/find_light_blobs.cpp b/armor/src/armor_finder/find/find_light_blobs.cpp new file mode 100644 index 0000000..c0bf173 --- /dev/null +++ b/armor/src/armor_finder/find/find_light_blobs.cpp @@ -0,0 +1,157 @@ +// +// Created by xinyang on 19-7-10. +// + +#include +#include +#include + +static double lw_rate(const cv::RotatedRect &rect) { + return rect.size.height > rect.size.width ? + rect.size.height / rect.size.width : + rect.size.width / rect.size.height; +} + + +static double areaRatio(const std::vector &contour, const cv::RotatedRect &rect){ + return cv::contourArea(contour) / rect.size.area(); +} + +static bool isValidLightBlob(const std::vector &contour, const cv::RotatedRect &rect) { + return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) && +// (rect.size.area() < 3000) && + ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) || + (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6)); +} + +// 此函数可以有性能优化. +static uint8_t get_blob_color(const cv::Mat &src, const cv::RotatedRect &blobPos) { + auto region = blobPos.boundingRect(); + region.x -= fmax(2, region.width * 0.1); + region.y -= fmax(2, region.height * 0.05); + region.width += 2 * fmax(2, region.width * 0.1); + region.height += 2 * fmax(2, region.height * 0.05); + region &= cv::Rect(0, 0, src.cols, src.rows); + cv::Mat roi = src(region); + int red_cnt = 0, blue_cnt = 0; + for (int row = 0; row < roi.rows; row++) { + for (int col = 0; col < roi.cols; col++) { + red_cnt += roi.at(row, col)[2]; + blue_cnt += roi.at(row, col)[0]; + } + } + if (red_cnt > blue_cnt) { + return BLOB_RED; + } else { + return BLOB_BLUE; + } +} + +static float linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) { + return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x; +} + +/// Todo:性能优化后的函数（还有点问题） +static double get_blob_color_opt(const cv::Mat &src, cv::RotatedRect blobPos) { + int blue_cnt=0, red_cnt=0; + blobPos.size.height *= 1.05; + blobPos.size.width *= 1.1; + cv::Point2f corners[4]; + blobPos.points(corners); + sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) { + return p1.y < p2.y; + }); + for (int r = corners[0].y; r < corners[1].y; r++) { + auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1; + auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + red_cnt += src.at(r, c)[2]; + blue_cnt += src.at(r, c)[0]; + } + } + for (int r = corners[1].y; r < corners[2].y; r++) { + auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1; + auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + red_cnt += src.at(r, c)[2]; + blue_cnt += src.at(r, c)[0]; + } + } + for (int r = corners[2].y; r < corners[3].y; r++) { + auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1; + auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + red_cnt += src.at(r, c)[2]; + blue_cnt += src.at(r, c)[0]; + } + } + if (red_cnt > blue_cnt) { + return BLOB_RED; + } else { + return BLOB_BLUE; + } +} + +static void imagePreProcess(cv::Mat &src) { + static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + erode(src, src, kernel_erode); + + static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + dilate(src, src, kernel_dilate); + + static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + dilate(src, src, kernel_dilate2); + + static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + erode(src, src, kernel_erode2); + +// float alpha = 1.5; +// int beta = 0; +// src.convertTo(src, -1, alpha, beta); +} + +bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { + cv::Mat color_channel; + cv::Mat src_bin_light, src_bin_dim; + std::vector channels; // 通道拆分 + + cv::split(src, channels); /************************/ + if (enemy_color == ENEMY_BLUE){ /* */ + color_channel = channels[0]; /* 根据目标颜色进行通道提取 */ + }else if (enemy_color == ENEMY_RED){ /* */ + color_channel = channels[2]; /************************/ + } + + + + cv::threshold(color_channel, src_bin_light, 200, 255, CV_THRESH_BINARY); // 二值化对应通道 + imagePreProcess(src_bin_light); // 开闭运算 + + cv::threshold(color_channel, src_bin_dim, 160, 255, CV_THRESH_BINARY); // 二值化对应通道 + imagePreProcess(src_bin_dim); // 开闭运算 + + if(src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) { + imshow("bin_light", src_bin_light); + imshow("bin_dim", src_bin_dim); + } + + std::vector > light_contours_light, light_contours_dim; + cv::findContours(src_bin_light, light_contours_light, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + cv::findContours(src_bin_dim, light_contours_dim, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + for (auto &light_contour : light_contours_light) { + cv::RotatedRect rect = cv::minAreaRect(light_contour); + if (isValidLightBlob(light_contour, rect)) { + light_blobs.emplace_back(rect, get_blob_color(src, rect)); + } + } + for (auto &light_contour : light_contours_dim) { + cv::RotatedRect rect = cv::minAreaRect(light_contour); + if (isValidLightBlob(light_contour, rect)) { + light_blobs.emplace_back(rect, get_blob_color(src, rect)); + } + } + return light_blobs.size() >= 2; +} \ No newline at end of file diff --git a/main.cpp b/main.cpp index a83077c..f3017a7 100644 --- a/main.cpp +++ b/main.cpp @@ -30,10 +30,10 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 - ENEMY_RED, // 敌方颜色 + ENEMY_BLUE, // 敌方颜色 }; WrapperHead *video_gimbal = nullptr; // 云台摄像头视频源 @@ -64,10 +64,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(0/*, "armor"*/); video_chassis = new CameraWrapper(1/*, "energy"*/); } else { -// video_gimbal = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi"); -// video_chassis = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi"); - video_gimbal = new VideoWrapper("/home/sjturm/Desktop/dafu/gimble3.avi"); - video_chassis = new VideoWrapper("/home/sjturm/Desktop/dafu/chassis3.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -93,19 +91,18 @@ int main(int argc, char *argv[]) { bool ok = true; cout << "start running" << endl; do { - CNT_TIME("Total", { - if (mcuData.state != ARMOR_STATE) {//能量机关模式 - if (last_state == ARMOR_STATE) {//若上一帧是自瞄模式，即刚往完成切换，则需要初始化 +// CNT_TIME("Total", { + if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 + if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 destroyAllWindows(); ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN); - energy.setEnergyInit(); + energy.setBigEnergyInit(); checkReconnect(video_chassis->read(chassis_src)); #ifdef CHASSIS_FLIP_MODE flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); #endif } ok = checkReconnect(video_gimbal->read(gimbal_src)); - video_gimbal->read(gimbal_src); video_chassis->read(chassis_src); #ifdef GIMBAL_FLIP_MODE flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); @@ -113,8 +110,22 @@ int main(int argc, char *argv[]) { if (!from_camera) extract(gimbal_src, chassis_src); if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 - energy.run(gimbal_src, chassis_src); -// energy.run(gimbal_src); + energy.runBig(gimbal_src, chassis_src); + last_state = mcuData.state;//更新上一帧状态 + } else if (mcuData.state == SMALL_ENERGY_STATE) { + if (mcuData.state != SMALL_ENERGY_STATE) { + destroyAllWindows(); + ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN); + energy.setSmallEnergyInit(); + } + ok = checkReconnect(video_gimbal->read(gimbal_src)); +#ifdef GIMBAL_FLIP_MODE + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); +#endif + if (!from_camera) extract(gimbal_src); + if (save_video) saveVideos(gimbal_src);//保存视频 + if (show_origin) showOrigin(gimbal_src);//显示原始图像 + energy.runSmall(gimbal_src); last_state = mcuData.state;//更新上一帧状态 } else { // 自瞄模式 if (last_state != ARMOR_STATE) { @@ -133,8 +144,8 @@ int main(int argc, char *argv[]) { armorFinder.run(gimbal_src); }); } - cv::waitKey(1); - }); +// cv::waitKey(1); +// }); } while (ok); delete video_gimbal; video_gimbal = nullptr;