summaryrefslogtreecommitdiffhomepage
path: root/tracker-pt
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-pt')
-rw-r--r--tracker-pt/ftnoir_tracker_pt_dialog.cpp8
-rw-r--r--tracker-pt/ftnoir_tracker_pt_dialog.h2
-rw-r--r--tracker-pt/point_extractor.cpp228
-rw-r--r--tracker-pt/point_extractor.h15
4 files changed, 130 insertions, 123 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.cpp b/tracker-pt/ftnoir_tracker_pt_dialog.cpp
index 98e8e424..c2079c27 100644
--- a/tracker-pt/ftnoir_tracker_pt_dialog.cpp
+++ b/tracker-pt/ftnoir_tracker_pt_dialog.cpp
@@ -76,15 +76,17 @@ TrackerDialog_PT::TrackerDialog_PT()
timer.setInterval(250);
connect(&calib_timer, &QTimer::timeout, this, &TrackerDialog_PT::trans_calib_step);
- calib_timer.setInterval(100);
+ calib_timer.setInterval(35);
poll_tracker_info_impl();
- connect(this, &TrackerDialog_PT::poll_tracker_info, this, &TrackerDialog_PT::poll_tracker_info_impl, Qt::QueuedConnection);
+ connect(this, &TrackerDialog_PT::poll_tracker_info, this, &TrackerDialog_PT::poll_tracker_info_impl, Qt::DirectConnection);
}
void TrackerDialog_PT::startstop_trans_calib(bool start)
{
+ QMutexLocker l(&calibrator_mutex);
+
if (start)
{
qDebug() << "pt: starting translation calibration";
@@ -174,6 +176,8 @@ void TrackerDialog_PT::show_camera_settings()
void TrackerDialog_PT::trans_calib_step()
{
+ QMutexLocker l(&calibrator_mutex);
+
if (tracker)
{
Affine X_CM = tracker->pose();
diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.h b/tracker-pt/ftnoir_tracker_pt_dialog.h
index 2932252c..59519601 100644
--- a/tracker-pt/ftnoir_tracker_pt_dialog.h
+++ b/tracker-pt/ftnoir_tracker_pt_dialog.h
@@ -16,6 +16,7 @@
#include "cv/video-widget.hpp"
#include <QTimer>
+#include <QMutex>
class TrackerDialog_PT : public ITrackerDialog
{
@@ -41,6 +42,7 @@ private:
Tracker_PT* tracker;
QTimer timer, calib_timer;
TranslationCalibrator trans_calib;
+ QMutex calibrator_mutex;
Ui::UICPTClientControls ui;
};
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index 5cefcbcd..a67a72df 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -12,10 +12,13 @@
#include <QDebug>
#include <opencv2/videoio.hpp>
+#include <opencv2/imgproc.hpp>
#include <cmath>
#include <algorithm>
#include <cinttypes>
+#include <vector>
+#include <array>
using std::sqrt;
using std::fmax;
@@ -84,12 +87,11 @@ void PointExtractor::extract_points(const cv::Mat& frame, cv::Mat& preview_frame
{
frame_gray = cv::Mat(frame.rows, frame.cols, CV_8U);
frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);
- frame_blobs = cv::Mat(frame.rows, frame.cols, CV_8U);
+ //frame_blobs = cv::Mat(frame.rows, frame.cols, CV_8U);
}
- // convert to grayscale
-
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
+
const double region_size_min = s.min_point_size;
const double region_size_max = s.max_point_size;
@@ -100,160 +102,156 @@ void PointExtractor::extract_points(const cv::Mat& frame, cv::Mat& preview_frame
}
else
{
- cv::calcHist(std::vector<cv::Mat> { frame_gray },
- std::vector<int> { 0 },
- cv::Mat(),
+ static const std::vector<int> used_channels { 0 };
+ static const std::vector<int> hist_size { 256 };
+ static const std::vector<float> hist_ranges { 0, 256 };
+
+ cv::calcHist(std::vector<cv::Mat1b> { frame_gray },
+ used_channels,
+ cv::noArray(),
hist,
- std::vector<int> { 256 },
- std::vector<float> { 0, 256 },
+ hist_size,
+ hist_ranges,
false);
static constexpr double min_radius = 2.5;
static constexpr double max_radius = 15;
+ const float* restrict ptr = reinterpret_cast<const float*>(hist.data);
const double radius = fmax(0., (max_radius-min_radius) * s.threshold / 255 + min_radius);
- const float* OTR_RESTRICT ptr = reinterpret_cast<const float*>(hist.ptr(0));
- const unsigned area = uround(3 * M_PI * radius*radius);
- const unsigned sz = unsigned(hist.cols * hist.rows);
- unsigned thres = 1;
- for (unsigned i = sz-1, cnt = 0; i > 1; i--)
+ const unsigned area = uround(3 * M_PI * radius * radius);
+ unsigned thres = 255;
+ unsigned accum = 0;
+
+ for (unsigned k = 255; k != 0; k--)
{
- cnt += ptr[i];
- if (cnt >= area)
+ accum += ptr[k];
+ if (accum >= area)
{
- thres = i;
+ thres = k;
break;
}
}
- //val *= 240./256.;
- //qDebug() << "thres" << thres;
- cv::threshold(frame_gray, frame_bin, thres, 255, CV_THRESH_BINARY);
+ cv::threshold(frame_gray, frame_bin, thres, 255, cv::THRESH_BINARY);
}
blobs.clear();
- frame_bin.copyTo(frame_blobs);
- unsigned idx = 0;
- for (int y=0; y < frame_blobs.rows; y++)
+ // -----
+ // start code borrowed from OpenCV's modules/features2d/src/blobdetector.cpp
+ // -----
+
+ contours.clear();
+
+ cv::findContours(frame_bin, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
+ const unsigned cnt = std::min(unsigned(max_blobs), contours.size());
+
+ for (unsigned k = 0; k < cnt; k++)
{
- const unsigned char* ptr_bin = frame_blobs.ptr(y);
- for (int x=0; x < frame_blobs.cols; x++)
- {
- if (ptr_bin[x] != 255)
- continue;
- idx = blobs.size() + 1;
-
- cv::Rect rect;
- cv::floodFill(frame_blobs,
- cv::Point(x,y),
- cv::Scalar(idx),
- &rect,
- cv::Scalar(0),
- cv::Scalar(0),
- 8);
-
- // these are doubles since m10 and m01 could overflow theoretically
- // log2(255^2 * 640^2 * pi) > 36
- double m10 = 0;
- double m01 = 0;
- // norm can't overflow since there's no 640^2 component
- int norm = 0;
- int cnt = 0;
-
- for (int i=rect.y; i < (rect.y+rect.height); i++)
- {
- unsigned char* ptr_blobs = frame_blobs.ptr(i);
- const unsigned char* ptr_gray = frame_gray.ptr(i);
- for (int j=rect.x; j < (rect.x+rect.width); j++)
- {
- if (ptr_blobs[j] != idx)
- continue;
-
- ptr_blobs[j] = 0;
-
- // square as a weight gives better results
- const int val(int(ptr_gray[j]) * int(ptr_gray[j]));
-
- norm += val;
- m01 += i * val;
- m10 += j * val;
- cnt++;
- }
- }
- if (norm > 0)
- {
- const double radius = sqrt(cnt / M_PI), N = double(norm);
- if (radius > region_size_max || radius < region_size_min)
- continue;
-
- blob b(radius, cv::Vec2d(m10 / N, m01 / N), N/sqrt(double(cnt)), rect);
- blobs.push_back(b);
-
- {
- static const f offx = 10, offy = 7.5;
- const f cx = preview_frame.cols / f(frame.cols),
- cy = preview_frame.rows / f(frame.rows),
- c_ = (cx+cy)/2;
- cv::Point p(iround(b.pos[0] * cx), iround(b.pos[1] * cy));
-
- cv::circle(preview_frame, p, iround((b.radius-2) * c_), cv::Scalar(255, 255, 0), 1, cv::LINE_AA);
- cv::circle(preview_frame, p, 1, cv::Scalar(255, 255, 64), -1, cv::LINE_AA);
-
- char buf[64];
- sprintf(buf, "%.1fpx", radius);
-
- cv::putText(preview_frame,
- buf,
- cv::Point(iround(b.pos[0]*cx+offx), iround(b.pos[1]*cy+offy)),
- cv::FONT_HERSHEY_PLAIN,
- 1,
- cv::Scalar(0, 0, 255),
- 1);
- }
-
- if (idx >= max_blobs) goto end;
- }
- }
+ if (contours[k].size() == 0)
+ continue;
+
+ cv::Moments moments = cv::moments(contours[k]);
+
+ const double area = moments.m00;
+ const double radius = std::sqrt(area) / std::sqrt(M_PI);
+
+ if (radius < std::fmax(2.5, region_size_min) || (radius > region_size_max))
+ continue;
+
+ cv::Rect rect = cv::boundingRect(contours[k]) & cv::Rect(0, 0, frame.cols, frame.rows);
+
+ rect &= cv::Rect(0, 0, frame.cols, frame.rows); // crop at frame boundaries
+
+ if (rect.width == 0 || rect.height == 0)
+ continue;
+
+ const vec2 center(moments.m10 / moments.m00, moments.m01 / moments.m00);
+
+ if (!cv::Point2d(center).inside(cv::Rect2d(rect)))
+ continue;
+
+ const double value = radius;
+
+ blob b(radius, center, value, rect);
+
+ blobs.push_back(b);
+
+ static const f offx = 10, offy = 7.5;
+ const f cx = preview_frame.cols / f(frame.cols),
+ cy = preview_frame.rows / f(frame.rows),
+ c_ = (cx+cy)/2;
+
+ static constexpr unsigned fract_bits = 16;
+ static constexpr double c_fract(1 << fract_bits);
+
+ cv::Point p(iround(b.pos[0] * cx * c_fract), iround(b.pos[1] * cy * c_fract));
+
+ cv::circle(preview_frame, p, iround((b.radius + 2) * c_ * c_fract), cv::Scalar(255, 255, 0), 1, cv::LINE_AA, fract_bits);
+ cv::circle(preview_frame, p, 1, cv::Scalar(255, 255, 64), -1, cv::LINE_4);
+
+ char buf[64];
+ sprintf(buf, "%.1fpx", int(b.radius*10+.5)/10.);
+
+ cv::putText(preview_frame,
+ buf,
+ cv::Point(iround(b.pos[0]*cx+offx), iround(b.pos[1]*cy+offy)),
+ cv::FONT_HERSHEY_PLAIN,
+ 1,
+ cv::Scalar(0, 0, 255),
+ 1);
}
-end:
+ // -----
+ // end of code borrowed from OpenCV's modules/features2d/src/blobdetector.cpp
+ // -----
- std::sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) -> bool { return b2.brightness < b1.brightness; });
+ std::sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) { return b2.value < b1.value; });
const int W = frame.cols;
const int H = frame.rows;
- for (idx = 0; idx < min(PointModel::N_POINTS, unsigned(blobs.size())); ++idx)
- {
- blob &b = blobs[idx];
- cv::Rect rect = b.rect;
+#if defined DEBUG_MEANSHIFT
+ double meanshift_total = 0;
+#endif
- rect.x -= rect.width / 2;
- rect.y -= rect.height / 2;
- rect.width *= 2;
- rect.height *= 2;
- rect &= cv::Rect(0, 0, W, H); // crop at frame boundaries
+ for (unsigned k = 0; k < min(PointModel::N_POINTS, unsigned(blobs.size())); ++k)
+ {
+ blob &b = blobs[k];
+ const cv::Rect rect = b.rect;
cv::Mat frame_roi = frame_gray(rect);
- static constexpr f radius_c = 1.5;
+ static constexpr f radius_c = 1.75;
const f kernel_radius = b.radius * radius_c;
cv::Vec2d pos(b.pos[0] - rect.x, b.pos[1] - rect.y); // position relative to ROI.
+ cv::Vec2d pos_(pos);
for (int iter = 0; iter < 10; ++iter)
{
cv::Vec2d com_new = MeanShiftIteration(frame_roi, pos, kernel_radius);
cv::Vec2d delta = com_new - pos;
pos = com_new;
- if (delta.dot(delta) < 1e-2)
+ if (delta.dot(delta) < 1e-3)
break;
}
+#if defined DEBUG_MEANSHIFT
+ meanshift_total += std::sqrt((pos_ - pos).dot(pos_ - pos));
+#endif
+
b.pos[0] = pos[0] + rect.x;
b.pos[1] = pos[1] + rect.y;
+
+ if (!cv::Point2d(b.pos[0], b.pos[1]).inside(b.rect))
+ continue;
}
+#if defined DEBUG_MEANSHIFT
+ qDebug() << "meanshift adjust total" << meanshift_total;
+#endif
+
// End of mean shift code. At this point, blob positions are updated with hopefully less noisy, less biased values.
points.reserve(max_blobs);
points.clear();
@@ -267,8 +265,8 @@ end:
}
}
-blob::blob(double radius, const cv::Vec2d& pos, double brightness, cv::Rect& rect) :
- radius(radius), brightness(brightness), pos(pos), rect(rect)
+blob::blob(double radius, const cv::Vec2d& pos, double brightness, const cv::Rect& rect) :
+ radius(radius), value(brightness), pos(pos), rect(rect)
{
//qDebug() << "radius" << radius << "pos" << pos[0] << pos[1];
}
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 0b179376..ac632b74 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -17,17 +17,19 @@
#include <vector>
+//#define DEBUG_MEANSHIFT
+
namespace pt_extractor_impl {
using namespace types;
struct blob
{
- double radius, brightness;
+ double radius, value;
vec2 pos;
cv::Rect rect;
- blob(double radius, const cv::Vec2d& pos, double brightness, cv::Rect &rect);
+ blob(double radius, const cv::Vec2d& pos, double value, const cv::Rect &rect);
};
class PointExtractor final
@@ -42,12 +44,13 @@ public:
private:
static constexpr int max_blobs = 16;
- cv::Mat frame_gray;
- cv::Mat frame_bin;
- cv::Mat hist;
- cv::Mat frame_blobs;
+ cv::Mat1b frame_bin, frame_gray;
+ //cv::Mat1b frame_blobs;
+ cv::Mat1f hist;
std::vector<blob> blobs;
+ std::vector<std::vector<cv::Point>> contours;
+ //std::vector<cv::Point> hull;
};
} // ns pt_extractor_impl
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-16 00:23:08 +0000