#ifndef BIG_BATCH
for (auto scale: p_scales)
for (auto angle : p_angles)
- d->threadctxs.emplace_back(feature_size, p_num_of_feats, scale, angle);
+ d->threadctxs.emplace_back(feature_size, (int)p_num_of_feats, scale, angle);
#else
- d->threadctxs.emplace_back(feature_size, p_num_of_feats, p_scales, p_angles);
+ d->threadctxs.emplace_back(feature_size, (int)p_num_of_feats, p_scales, p_angles);
#endif
gaussian_correlation.reset(new GaussianCorrelation(1, p_num_of_feats, feature_size));
}
}
+static void drawCross(cv::Mat &img, cv::Point center, bool green)
+{
+ cv::Scalar col = green ? cv::Scalar(0, 1, 0) : cv::Scalar(0, 0, 1);
+ cv::line(img, cv::Point(center.x, 0), cv::Point(center.x, img.size().height), col);
+ cv::line(img, cv::Point(0, center.y), cv::Point(img.size().height, center.y), col);
+}
+
+static cv::Point2d wrapAroundFreq(cv::Point2d pt, cv::Mat &resp_map)
+{
+ if (pt.y > resp_map.rows / 2) // wrap around to negative half-space of vertical axis
+ pt.y = pt.y - resp_map.rows;
+ if (pt.x > resp_map.cols / 2) // same for horizontal axis
+ pt.x = pt.x - resp_map.cols;
+ return pt;
+}
+
double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) const
{
double max;
assert(max_it != vec.end());
max = max_it->IF_BIG_BATCH(response, max.response);
- if (m_visual_debug) {
- const bool rgb = true;
- int type = rgb ? d->threadctxs[0].IF_BIG_BATCH(dbg_patch[0], dbg_patch).type()
- : d->threadctxs[0].response.type();
- int w = true ? 100 : (rgb ? fit_size.width : feature_size.width);
- int h = true ? 100 : (rgb ? fit_size.height : feature_size.height);
- cv::Mat all_responses((h + 1) * p_num_scales - 1,
- (w + 1) * p_num_angles - 1, type, cv::Scalar::all(0));
- for (size_t i = 0; i < p_num_scales; ++i) {
- for (size_t j = 0; j < p_num_angles; ++j) {
- cv::Mat tmp;
- if (rgb) {
- tmp = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j)).dbg_patch IF_BIG_BATCH((i, j),);
- } else {
- tmp = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j)).response.plane(IF_BIG_BATCH(d->threadctxs[0].max.getIdx(i, j), 0));
- tmp = circshift(tmp, -tmp.cols/2, -tmp.rows/2);
- }
- cv::resize(tmp, tmp, cv::Size(w, h));
- cv::Mat resp_roi(all_responses, cv::Rect(j * (w+1), i * (h+1), w, h));
- tmp.copyTo(resp_roi);
- }
- }
- cv::namedWindow("KCF visual debug", CV_WINDOW_AUTOSIZE);
- cv::imshow("KCF visual debug", all_responses);
- }
-
max_idx = std::distance(vec.begin(), max_it);
cv::Point2i max_response_pt = IF_BIG_BATCH(max_it->loc, max_it->max.loc);
DEBUG_PRINTM(max_response_map);
DEBUG_PRINT(max_response_pt);
- // sub pixel quadratic interpolation from neighbours
- if (max_response_pt.y > max_response_map.rows / 2) // wrap around to negative half-space of vertical axis
- max_response_pt.y = max_response_pt.y - max_response_map.rows;
- if (max_response_pt.x > max_response_map.cols / 2) // same for horizontal axis
- max_response_pt.x = max_response_pt.x - max_response_map.cols;
-
+ max_response_pt = wrapAroundFreq(max_response_pt, max_response_map);
+ // sub pixel quadratic interpolation from neighbours
if (m_use_subpixel_localization) {
new_location = sub_pixel_peak(max_response_pt, max_response_map);
} else {
new_location = max_response_pt;
}
DEBUG_PRINT(new_location);
+
+ if (m_visual_debug != vd::NONE) {
+ const bool fit = 1;
+ int w = fit ? 100 : (m_visual_debug == vd::PATCH ? fit_size.width : feature_size.width);
+ int h = fit ? 100 : (m_visual_debug == vd::PATCH ? fit_size.height : feature_size.height);
+ cv::Mat all_responses((h + 1) * p_num_scales - 1,
+ (w + 1) * p_num_angles - 1, CV_32FC3, cv::Scalar::all(0));
+ for (size_t i = 0; i < p_num_scales; ++i) {
+ for (size_t j = 0; j < p_num_angles; ++j) {
+ auto &threadctx = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j));
+ cv::Mat tmp;
+ cv::Point2d cross = threadctx.IF_BIG_BATCH(max(i, j), max).loc;
+ cross = wrapAroundFreq(cross, max_response_map);
+ if (m_visual_debug == vd::PATCH ) {
+ threadctx.dbg_patch IF_BIG_BATCH((i, j),)
+ .convertTo(tmp, all_responses.type(), 1.0 / 255);
+ cross.x = cross.x / fit_size.width * tmp.cols + tmp.cols / 2;
+ cross.y = cross.y / fit_size.height * tmp.rows + tmp.rows / 2;
+ } else {
+ cv::cvtColor(threadctx.response.plane(IF_BIG_BATCH(threadctx.max.getIdx(i, j), 0)),
+ tmp, cv::COLOR_GRAY2BGR);
+ tmp /= max; // Normalize to 1
+ cross += cv::Point2d(tmp.size())/2;
+ tmp = circshift(tmp, -tmp.cols/2, -tmp.rows/2);
+ //drawCross(tmp, cross, false);
+ }
+ bool green = false;
+ if (&*max_it == &IF_BIG_BATCH(threadctx.max(i, j), threadctx)) {
+ // Show the green cross at position of sub-pixel interpolation (if enabled)
+ cross = new_location + cv::Point2d(tmp.size())/2;
+ green = true;
+ }
+ // Move to the center of pixes (if scaling up) and scale
+ cross.x = (cross.x + 0.5) * double(w)/tmp.cols;
+ cross.y = (cross.y + 0.5) * double(h)/tmp.rows;
+ cv::resize(tmp, tmp, cv::Size(w, h)); //, 0, 0, cv::INTER_NEAREST);
+ drawCross(tmp, cross, green);
+ cv::Mat resp_roi(all_responses, cv::Rect(j * (w+1), i * (h+1), w, h));
+ tmp.copyTo(resp_roi);
+ }
+ }
+ cv::namedWindow("KCF visual debug", CV_WINDOW_AUTOSIZE);
+ cv::imshow("KCF visual debug", all_responses);
+ }
+
return max;
}
uint max_idx;
max_response = findMaxReponse(max_idx, new_location);
+ double angle_change = m_use_subgrid_angle ? sub_grid_angle(max_idx)
+ : d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).angle(max_idx);
+ p_current_angle += angle_change;
+
+ new_location.x = new_location.x * cos(-p_current_angle/180*M_PI) + new_location.y * sin(-p_current_angle/180*M_PI);
+ new_location.y = new_location.y * cos(-p_current_angle/180*M_PI) - new_location.x * sin(-p_current_angle/180*M_PI);
+
new_location.x *= double(p_windows_size.width) / fit_size.width;
new_location.y *= double(p_windows_size.height) / fit_size.height;
clamp2(p_current_scale, p_min_max_scale[0], p_min_max_scale[1]);
- p_current_angle += d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).angle(max_idx);
// train at newly estimated target position
train(input_rgb, input_gray, p_interp_factor);
for (size_t i = 0; i < max.size(); ++i) {
cv::minMaxLoc(response.plane(i), &min_val, &max_val, &min_loc, &max_loc);
DEBUG_PRINT(max_loc);
- double weight = kcf.p_scales[i] < 1. ? kcf.p_scales[i] : 1. / kcf.p_scales[i];
+ double weight = max.scale(i) < 1. ? max.scale(i) : 1. / max.scale(i);
max[i].response = max_val * weight;
max[i].loc = max_loc;
}
cv::Mat patch_gray = get_subwindow(input_gray, cx, cy, scaled.width, scaled.height, angle);
cv::Mat patch_rgb = get_subwindow(input_rgb, cx, cy, scaled.width, scaled.height, angle);
- if (dbg_patch)
- patch_rgb.copyTo(*dbg_patch);
-
// resize to default size
if (scaled.area() > fit_size.area()) {
// if we downsample use INTER_AREA interpolation
}
}
+ if (dbg_patch)
+ patch_rgb.copyTo(*dbg_patch);
+
if (m_use_color && input_rgb.channels() == 3) {
// use rgb color space
cv::Mat patch_rgb_norm;
cv::Mat KCF_Tracker::circshift(const cv::Mat &patch, int x_rot, int y_rot) const
{
- cv::Mat rot_patch(patch.size(), CV_32FC1);
- cv::Mat tmp_x_rot(patch.size(), CV_32FC1);
+ cv::Mat rot_patch(patch.size(), patch.type());
+ cv::Mat tmp_x_rot(patch.size(), patch.type());
// circular rotate x-axis
if (x_rot < 0) {
float a = x.at<float>(0), b = x.at<float>(1), c = x.at<float>(2), d = x.at<float>(3), e = x.at<float>(4);
cv::Point2f sub_peak(max_loc.x, max_loc.y);
- if (b > 0 || b < 0) {
+ if (4 * a * c - b * b > p_floating_error) {
sub_peak.y = ((2.f * a * e) / b - d) / (b - (4 * a * c) / b);
sub_peak.x = (-2 * c * sub_peak.y - e) / b;
+ if (fabs(sub_peak.x - max_loc.x) > 1 ||
+ fabs(sub_peak.y - max_loc.y) > 1)
+ sub_peak = max_loc;
}
return sub_peak;
cv::Mat A, fval;
const auto &vec = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs);
uint index = vec.getScaleIdx(max_index);
- uint angle_idx = vec.getAngleIdx(index);
+ uint angle_idx = vec.getAngleIdx(max_index);
if (index >= vec.size()) {
// interpolate from all values
scale = -b / (2 * a);
return scale;
}
+
+double KCF_Tracker::sub_grid_angle(uint max_index)
+{
+ cv::Mat A, fval;
+ const auto &vec = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs);
+ uint scale_idx = vec.getScaleIdx(max_index);
+ uint index = vec.getAngleIdx(max_index);
+
+ if (index >= vec.size()) {
+ // interpolate from all values
+ // fit 1d quadratic function f(x) = a*x^2 + b*x + c
+ A.create(p_angles.size(), 3, CV_32FC1);
+ fval.create(p_angles.size(), 1, CV_32FC1);
+ for (size_t i = 0; i < p_angles.size(); ++i) {
+ A.at<float>(i, 0) = float(p_angles[i] * p_angles[i]);
+ A.at<float>(i, 1) = float(p_angles[i]);
+ A.at<float>(i, 2) = 1;
+ fval.at<float>(i) = d->IF_BIG_BATCH(threadctxs[0].max[i].response, threadctxs(scale_idx, i).max.response);
+ }
+ } else {
+ // only from neighbours
+ if (index == 0 || index == p_angles.size() - 1)
+ return p_angles[index];
+
+ A = (cv::Mat_<float>(3, 3) <<
+ p_angles[index - 1] * p_angles[index - 1], p_angles[index - 1], 1,
+ p_angles[index + 0] * p_angles[index + 0], p_angles[index + 0], 1,
+ p_angles[index + 1] * p_angles[index + 1], p_angles[index + 1], 1);
+#ifdef BIG_BATCH
+ fval = (cv::Mat_<float>(3, 1) <<
+ d->threadctxs[0].max(scale_idx, index - 1).response,
+ d->threadctxs[0].max(scale_idx, index + 0).response,
+ d->threadctxs[0].max(scale_idx, index + 1).response);
+#else
+ fval = (cv::Mat_<float>(3, 1) <<
+ d->threadctxs(scale_idx, index - 1).max.response,
+ d->threadctxs(scale_idx, index + 0).max.response,
+ d->threadctxs(scale_idx, index + 1).max.response);
+#endif
+ }
+
+ cv::Mat x;
+ cv::solve(A, fval, x, cv::DECOMP_SVD);
+ float a = x.at<float>(0), b = x.at<float>(1);
+ double angle = p_angles[index];
+ if (a > 0 || a < 0)
+ angle = -b / (2 * a);
+ return angle;
+}
projects / hercules2020 / kcf.git / blobdiff