From 14db8f3384e72d10952da882cd0dc111c582cc45 Mon Sep 17 00:00:00 2001 From: AlexeyAB Date: Mon, 12 Feb 2018 22:36:52 +0300 Subject: [PATCH] Added linear extrapolation of coordinates --- src/yolo_console_dll.cpp | 168 ++++++++++++++++++++++++++++++++++++--- src/yolo_v2_class.hpp | 3 +- 2 files changed, 161 insertions(+), 10 deletions(-) diff --git a/src/yolo_console_dll.cpp b/src/yolo_console_dll.cpp index 686ea33c..3bd1246b 100644 --- a/src/yolo_console_dll.cpp +++ b/src/yolo_console_dll.cpp @@ -14,7 +14,7 @@ #endif // To use tracking - uncomment the following line. Tracking is supported only by OpenCV 3.x -//#define TRACK_OPTFLOW +#define TRACK_OPTFLOW #include "yolo_v2_class.hpp" // imported functions from DLL @@ -37,6 +37,140 @@ #pragma comment(lib, "opencv_highgui" OPENCV_VERSION ".lib") #endif +class track_kalman { +public: + cv::KalmanFilter kf; + int state_size, meas_size, contr_size; + + + track_kalman(int _state_size = 10, int _meas_size = 10, int _contr_size = 0) + : state_size(_state_size), meas_size(_meas_size), contr_size(_contr_size) + { + kf.init(state_size, meas_size, contr_size, CV_32F); + + cv::setIdentity(kf.measurementMatrix); + cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1)); + cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-5)); + cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1e-2)); + cv::setIdentity(kf.transitionMatrix); + } + + void set(std::vector result_vec) { + for (size_t i = 0; i < result_vec.size() && i < state_size*2; ++i) { + kf.statePost.at(i * 2 + 0) = result_vec[i].x; + kf.statePost.at(i * 2 + 1) = result_vec[i].y; + } + } + + // Kalman.correct() calculates: statePost = statePre + gain * (z(k)-measurementMatrix*statePre); + // corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) + std::vector correct(std::vector result_vec) { + cv::Mat measurement(meas_size, 1, CV_32F); + for (size_t i = 0; i < result_vec.size() && i < meas_size * 2; ++i) { + measurement.at(i * 2 + 0) = result_vec[i].x; + measurement.at(i * 2 + 1) = result_vec[i].y; + } + cv::Mat estimated = kf.correct(measurement); + for (size_t i = 0; i < result_vec.size() && i < meas_size * 2; ++i) { + result_vec[i].x = estimated.at(i * 2 + 0); + result_vec[i].y = estimated.at(i * 2 + 1); + } + return result_vec; + } + + // Kalman.predict() calculates: statePre = TransitionMatrix * statePost; + // predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) + std::vector predict() { + std::vector result_vec; + cv::Mat control; + cv::Mat prediction = kf.predict(control); + for (size_t i = 0; i < prediction.rows && i < state_size * 2; ++i) { + result_vec[i].x = prediction.at(i * 2 + 0); + result_vec[i].y = prediction.at(i * 2 + 1); + } + return result_vec; + } + +}; + + + + +class extrapolate_coords_t { +public: + std::vector old_result_vec; + std::vector dx_vec, dy_vec, time_vec; + std::vector old_dx_vec, old_dy_vec; + + void new_result(std::vector new_result_vec, float new_time) { + old_dx_vec = dx_vec; + old_dy_vec = dy_vec; + if (old_dx_vec.size() != old_result_vec.size()) std::cout << "old_dx != old_res \n"; + dx_vec = std::vector(new_result_vec.size(), 0); + dy_vec = std::vector(new_result_vec.size(), 0); + update_result(new_result_vec, new_time, false); + old_result_vec = new_result_vec; + time_vec = std::vector(new_result_vec.size(), new_time); + } + + void update_result(std::vector new_result_vec, float new_time, bool update = true) { + for (size_t i = 0; i < new_result_vec.size(); ++i) { + for (size_t k = 0; k < old_result_vec.size(); ++k) { + if (old_result_vec[k].track_id == new_result_vec[i].track_id && old_result_vec[k].obj_id == new_result_vec[i].obj_id) { + float const delta_time = new_time - time_vec[k]; + if (abs(delta_time) < 1) break; + size_t index = (update) ? k : i; + float dx = ((float)new_result_vec[i].x - (float)old_result_vec[k].x) / delta_time; + float dy = ((float)new_result_vec[i].y - (float)old_result_vec[k].y) / delta_time; + float old_dx = dx, old_dy = dy; + + // if it's shaking + if (update) { + if (dx * dx_vec[i] < 0) dx = dx / 2; + if (dy * dy_vec[i] < 0) dy = dy / 2; + } else { + if (dx * old_dx_vec[k] < 0) dx = dx / 2; + if (dy * old_dy_vec[k] < 0) dy = dy / 2; + } + dx_vec[index] = dx; + dy_vec[index] = dy; + + //if (old_dx == dx && old_dy == dy) std::cout << "not shakin \n"; + //else std::cout << "shakin \n"; + + if (dx_vec[index] > 1000 || dy_vec[index] > 1000) { + //std::cout << "!!! bad dx or dy, dx = " << dx_vec[index] << ", dy = " << dy_vec[index] << + // ", delta_time = " << delta_time << ", update = " << update << std::endl; + dx_vec[index] = 0; + dy_vec[index] = 0; + } + old_result_vec[k].x = new_result_vec[i].x; + old_result_vec[k].y = new_result_vec[i].y; + time_vec[k] = new_time; + break; + } + } + } + } + + std::vector predict(float cur_time) { + std::vector result_vec = old_result_vec; + for (size_t i = 0; i < old_result_vec.size(); ++i) { + float const delta_time = cur_time - time_vec[i]; + auto &bbox = result_vec[i]; + float new_x = (float) bbox.x + dx_vec[i] * delta_time; + float new_y = (float) bbox.y + dy_vec[i] * delta_time; + if (new_x > 0) bbox.x = new_x; + else bbox.x = 0; + if (new_y > 0) bbox.y = new_y; + else bbox.y = 0; + } + return result_vec; + } + +}; + + void draw_boxes(cv::Mat mat_img, std::vector result_vec, std::vector obj_names, int current_det_fps = -1, int current_cap_fps = -1) { @@ -104,7 +238,7 @@ int main(int argc, char *argv[]) auto obj_names = objects_names_from_file(names_file); std::string out_videofile = "result.avi"; - bool const save_output_videofile = false; + bool const save_output_videofile = true; #ifdef TRACK_OPTFLOW Tracker_optflow tracker_flow; detector.wait_stream = true; @@ -118,6 +252,9 @@ int main(int argc, char *argv[]) try { #ifdef OPENCV + extrapolate_coords_t extrapolate_coords; + bool extrapolate_flag = false; + float cur_time_extrapolate = 0, old_time_extrapolate = 0; preview_boxes_t large_preview(100, 150, false), small_preview(50, 50, true); bool show_small_boxes = false; @@ -159,6 +296,7 @@ int main(int argc, char *argv[]) cur_frame = cap_frame.clone(); } t_cap = std::thread([&]() { cap >> cap_frame; }); + ++cur_time_extrapolate; // swap result bouned-boxes and input-frame if(consumed) @@ -177,18 +315,23 @@ int main(int argc, char *argv[]) while (track_optflow_queue.size() > 1) { track_optflow_queue.pop(); - result_vec = tracker_flow.tracking_flow(track_optflow_queue.front(), false); + result_vec = tracker_flow.tracking_flow(track_optflow_queue.front(), true); } track_optflow_queue.pop(); passed_flow_frames = 0; result_vec = detector.tracking_id(result_vec); - auto tmp_result_vec = detector.tracking_id(detected_result_vec); + auto tmp_result_vec = detector.tracking_id(detected_result_vec, false); small_preview.set(first_frame, tmp_result_vec); + + extrapolate_coords.new_result(tmp_result_vec, old_time_extrapolate); + old_time_extrapolate = cur_time_extrapolate; + extrapolate_coords.update_result(result_vec, cur_time_extrapolate - 1); } -#endif +#else result_vec = detector.tracking_id(result_vec); // comment it - if track_id is not required - + extrapolate_coords.new_result(result_vec, cur_time_extrapolate - 1); +#endif // add old tracked objects for (auto &i : old_result_vec) { auto it = std::find_if(result_vec.begin(), result_vec.end(), @@ -204,7 +347,7 @@ int main(int argc, char *argv[]) } #ifdef TRACK_OPTFLOW tracker_flow.update_cur_bbox_vec(result_vec); - result_vec = tracker_flow.tracking_flow(cur_frame, false); // track optical flow + result_vec = tracker_flow.tracking_flow(cur_frame, true); // track optical flow #endif consumed = false; cv_pre_tracked.notify_all(); @@ -245,16 +388,23 @@ int main(int argc, char *argv[]) ++passed_flow_frames; track_optflow_queue.push(cur_frame.clone()); result_vec = tracker_flow.tracking_flow(cur_frame); // track optical flow + extrapolate_coords.update_result(result_vec, cur_time_extrapolate); small_preview.draw(cur_frame, show_small_boxes); #endif - - draw_boxes(cur_frame, result_vec, obj_names, current_det_fps, current_cap_fps); + auto result_vec_draw = result_vec; + if (extrapolate_flag) { + result_vec_draw = extrapolate_coords.predict(cur_time_extrapolate); + cv::putText(cur_frame, "extrapolate", cv::Point2f(10, 40), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(50, 50, 0), 2); + } + draw_boxes(cur_frame, result_vec_draw, obj_names, current_det_fps, current_cap_fps); //show_console_result(result_vec, obj_names); large_preview.draw(cur_frame); cv::imshow("window name", cur_frame); int key = cv::waitKey(3); // 3 or 16ms if (key == 'f') show_small_boxes = !show_small_boxes; + if (key == 'p') while (true) if(cv::waitKey(100) == 'p') break; + if (key == 'e') extrapolate_flag = !extrapolate_flag; if (output_video.isOpened() && videowrite_ready) { if (t_videowrite.joinable()) t_videowrite.join(); diff --git a/src/yolo_v2_class.hpp b/src/yolo_v2_class.hpp index 7d0516ee..5aa36313 100644 --- a/src/yolo_v2_class.hpp +++ b/src/yolo_v2_class.hpp @@ -297,7 +297,8 @@ public: float moved_y = cur_key_pt.y - prev_key_pt.y; if (abs(moved_x) < 100 && abs(moved_y) < 100 && good_bbox_vec_flags[i]) - if (err_cpu.at(0, i) < flow_error && status_cpu.at(0, i) != 0) + if (err_cpu.at(0, i) < flow_error && status_cpu.at(0, i) != 0 && + ((float)cur_bbox_vec[i].x + moved_x) > 0 && ((float)cur_bbox_vec[i].y + moved_y) > 0) { cur_bbox_vec[i].x += moved_x + 0.5; cur_bbox_vec[i].y += moved_y + 0.5;