diff options
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 4 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.cpp | 234 |
2 files changed, 73 insertions, 165 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 4fa70ccf..19d1bd7f 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -93,6 +93,10 @@ void Tracker_PT::run() QMutexLocker lock(&mutex); std::vector<cv::Vec2f> points = point_extractor.extract_points(frame); + + // blobs are sorted in order of circularity + if (points.size() > PointModel::N_POINTS) + points.resize(PointModel::N_POINTS); bool success = points.size() == PointModel::N_POINTS; diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index c1dd9a54..fcdbbaed 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -28,215 +28,119 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame) cv::Mat frame_gray; cv::cvtColor(frame, frame_gray, cv::COLOR_RGB2GRAY); - int min_size = s.min_point_size; - int max_size = s.max_point_size; + const int region_size_min = s.min_point_size; + const int region_size_max = s.max_point_size; - unsigned int region_size_min = 3.14*min_size*min_size/4.0; - unsigned int region_size_max = 3.14*max_size*max_size/4.0; - - // testing indicates threshold difference of 45 from lowest to highest - // that's applicable to poor lighting conditions. - - static constexpr int diff = 20; - static constexpr int steps = 5; - static constexpr int successes = 5; - - int thres = s.threshold; - - struct blob { - std::vector<cv::Vec2d> pos; - std::vector<double> confids; - std::vector<double> areas; - - cv::Vec2d effective_pos() const - { - double x = 0; - double y = 0; - double norm = 0; - for (unsigned i = 0; i < pos.size(); i++) - { - const double w = confids[i] * areas[i]; - x += pos[i][0] * w; - y += pos[i][1] * w; - norm += w; - } - cv::Vec2d ret(x, y); - ret *= 1./norm; - return ret; - } - - double effective_area() const - { - double norm = 0, ret = 0; - for (unsigned i = 0; i < areas.size(); i++) - { - const double w = confids[i]; - norm += w; - ret += w * areas[i]; - } - return ret/norm; - } - }; + const int thres = s.threshold; struct simple_blob { - double radius_2; + double radius; cv::Vec2d pos; double confid; bool taken; double area; - simple_blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius_2(radius*radius), pos(pos), confid(confid), taken(false), area(area) + simple_blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area) { //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid; } bool inside(const simple_blob& other) { cv::Vec2d tmp = pos - other.pos; - return tmp.dot(tmp) < radius_2; - } - static std::vector<blob> merge(cv::Mat& frame, std::vector<simple_blob>& blobs) - { -#ifdef DEBUG_EXTRACTION - static Timer t; - bool debug = t.elapsed_ms() > 100; - if (debug) t.start(); -#endif - - std::vector<blob> ret; - for (unsigned i = 0; i < blobs.size(); i++) - { - auto& b = blobs[i]; - if (b.taken) - continue; - b.taken = true; - blob b_; - b_.pos.push_back(b.pos); - b_.confids.push_back(b.confid); - b_.areas.push_back(b.area); - - for (unsigned j = i+1; j < blobs.size(); j++) - { - auto& b2 = blobs[j]; - if (b2.taken) - continue; - if (b.inside(b2) || b2.inside(b)) - { - b2.taken = true; - b_.pos.push_back(b2.pos); - b_.confids.push_back(b2.confid); - b_.areas.push_back(b2.area); - } - } - if (b_.pos.size() >= successes) - ret.push_back(b_); - - char buf[64]; - sprintf(buf, "%d%% %d px", (int)(b_.pos.size()*100/successes), (int)(2.*sqrt(b_.effective_area()) / sqrt(3.14))); - const auto pos = b_.effective_pos(); - cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1); - } -#ifdef DEBUG_EXTRACTION - if (debug) - { - double diff = 0; - for (unsigned j = 0; j < ret.size(); j++) - { - auto& b = ret[j]; - cv::Vec2d pos = b.effective_pos(); - for (unsigned i = 0; i < b.pos.size(); i++) - { - auto tmp = pos - b.pos[i]; - diff += std::abs(tmp.dot(tmp)); - } - } - qDebug() << "diff" << diff; - } -#endif - return ret; + return sqrt(tmp.dot(tmp)) < radius; } }; // mask for everything that passes the threshold (or: the upper threshold of the hysteresis) cv::Mat frame_bin = cv::Mat::zeros(H, W, CV_8U); - const int min = std::max(0, thres - diff/2); - const int max = std::min(255, thres + diff/2); - const int step = std::max(1, diff / steps); - std::vector<simple_blob> blobs; - - // this code is based on OpenCV SimpleBlobDetector - for (int i = min; i < max; i += step) + std::vector<std::vector<cv::Point>> contours; { cv::Mat frame_bin_; - cv::threshold(frame_gray, frame_bin_, i, 255, cv::THRESH_BINARY); + cv::threshold(frame_gray, frame_bin_, thres, 255, cv::THRESH_BINARY); frame_bin.setTo(170, frame_bin_); - - std::vector<std::vector<cv::Point>> contours; cv::findContours(frame_bin_, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE); - - int cnt = 0; - - for (auto& c : contours) + } + + int cnt = 0; + + for (auto& c : contours) + { + if (cnt++ > 30) + break; + + const auto m = cv::moments(cv::Mat(c)); + const double area = m.m00; + if (area == 0.) + continue; + const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00); + + double radius; +// following based on OpenCV SimpleBlobDetector { - if (cnt++ > 30) - break; - - auto m = cv::moments(cv::Mat(c)); - const double area = m.m00; - if (area == 0.) - continue; - cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00); - if (area < region_size_min || area > region_size_max) - continue; - - double radius = 0; - + std::vector<double> dists; for (auto& k : c) { - cv::Vec2d pos_(k.x, k.y); - cv::Vec2d tmp = pos_ - pos; - radius = std::max(radius, sqrt(1e-2 + tmp.dot(tmp))); + dists.push_back(cv::norm(pos - cv::Vec2d(k.x, k.y))); } - double confid = 1; + std::sort(dists.begin(), dists.end()); + radius = (dists[(dists.size() - 1)/2] + dists[dists.size()/2])/2; + } + + if (radius < region_size_min || radius > region_size_max) + continue; + + double confid = 1; + { + double denominator = std::sqrt(std::pow(2 * m.mu11, 2) + std::pow(m.mu20 - m.mu02, 2)); + const double eps = 1e-2; + if (denominator > eps) { - double denominator = std::sqrt(std::pow(2 * m.mu11, 2) + std::pow(m.mu20 - m.mu02, 2)); - const double eps = 1e-2; - if (denominator > eps) - { - double cosmin = (m.mu20 - m.mu02) / denominator; - double sinmin = 2 * m.mu11 / denominator; - double cosmax = -cosmin; - double sinmax = -sinmin; - - double imin = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmin - m.mu11 * sinmin; - double imax = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmax - m.mu11 * sinmax; - confid = imin / imax; - } + double cosmin = (m.mu20 - m.mu02) / denominator; + double sinmin = 2 * m.mu11 / denominator; + double cosmax = -cosmin; + double sinmax = -sinmin; + + double imin = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmin - m.mu11 * sinmin; + double imax = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmax - m.mu11 * sinmax; + confid = imin / imax; } - blobs.push_back(simple_blob(radius, pos, confid, area)); } +// end SimpleBlobDetector + + { + char buf[64]; + sprintf(buf, "%.2fpx %.2fc", radius, confid); + cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1); + } + + blobs.push_back(simple_blob(radius, pos, confid, area)); } // clear old points points.clear(); + + using b = const simple_blob; + std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;}); - for (auto& b : simple_blob::merge(frame, blobs)) + for (auto& b : blobs) { - auto pos = b.effective_pos(); - cv::Vec2f p((pos[0] - W/2)/W, -(pos[1] - H/2)/W); + cv::Vec2f p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W); points.push_back(p); } + // draw output image std::vector<cv::Mat> channels_; cv::split(frame, channels_); - // draw output image - cv::Mat frame_bin_ = frame_bin * .5; std::vector<cv::Mat> channels; - channels.push_back(channels_[0] + frame_bin_); - channels.push_back(channels_[1] - frame_bin_); - channels.push_back(channels_[2] - frame_bin_); - cv::merge(channels, frame); + { + cv::Mat frame_bin__ = frame_bin * .5; + channels.push_back(channels_[0] + frame_bin__); + channels.push_back(channels_[1] - frame_bin__); + channels.push_back(channels_[2] - frame_bin__); + cv::merge(channels, frame); + } return points; } |