void MT::fast_train(Warp warp)
{
Rect2f rect = window(warp.t);
float fast_stride = sqrt(rect.area() / fast_n);
feature.set_cell(fast_stride);
int W = int(floor(rect.width * 0.5f / fast_stride));
int H = int(floor(rect.height * 0.5f / fast_stride));
int ox = int(round(rect.width * 0.5f));
int oy = int(round(rect.height * 0.5f));
int stride = int(round(fast_stride));
fast_samples.clear();
for (int y = 0; y <= 2 * H; ++y)
for (int x = 0; x <= 2 * W; ++x)
fast_samples.push_back(Point(ox + (x - W) * stride, oy + (y - H) * stride));
fast_model.create(fast_samples.size(), L, CV_32FC1);
int x = int(round(rect.x));
int y = int(round(rect.y));
for (int i = 0; i < fast_samples.size(); ++i) {
int tx = x + fast_samples[i].x;
int ty = y + fast_samples[i].y;
float *dst = fast_model.ptr<float>(i);
float *src = feature.cell_hist(tx, ty);
memcpy(dst, src, L * sizeof(float));
}
}
Vector2f clamp( const Vector2f& v, const Rect2f& rect )
{
assert( rect.isStandard() );
return
{
clampToRangeInclusive( v.x, rect.left(), rect.right() ),
clampToRangeInclusive( v.y, rect.bottom(), rect.top() )
};
}
float overlap(Rect2f a, Rect2f b)
{
if (a.width <= 0.0f || a.height <= 0.0f || a.area() <= 0.0f)
return 0.0f;
if (b.width <= 0.0f || b.height <= 0.0f || b.area() <= 0.0f)
return 0.0f;
float s = (a & b).area();
return s / (a.area() + b.area() - s);
}
Warp MT::fine_test(Warp warp)
{
Rect2f rect = window(warp.t);
float fine_cell = sqrt(rect.area() / cell_n);
feature.set_cell(fine_cell);
for (auto fine_step : fine_steps) {
if (fine_step > 2.0f * fine_cell)
continue;
feature.set_step(fine_step);
if (log != NULL)
(*log) << "\tcell = " << fine_cell << " step = " << fine_step << endl;
warp = Lucas_Kanade(warp);
}
return warp;
}
Point3f MT::locate(Rect2f rect)
{
float scale = sqrt(window_size.area() / rect.area());
float x = rect.x + rect.width * 0.5f - warp.c.x;
float y = rect.y + rect.height * 0.5f - warp.c.y;
return Point3f(x, y, warp.f) * scale;
}
float MT::evaluate(Warp warp)
{
Rect2f rect = window(warp.t);
float fine_cell = sqrt(rect.area() / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Matx<float, L4, 1> T(fine_model.ptr<float>(i)), I;
Point2f p = warp.transform2(fine_samples[i]);
feature.descriptor4(p.x, p.y, I.val);
T -= I;
E = E + sigmoid(T.dot(T));
}
return E / fine_samples.size();
}
void MT::fine_train(Warp warp)
{
Rect2f rect = window(warp.t);
float fine_cell = sqrt(rect.area() / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
Mat model(fine_samples.size(), L4, CV_32FC1);
for (int i = 0; i < fine_samples.size(); ++i) {
Point2f p = warp.transform2(fine_samples[i]);
feature.descriptor4(p.x, p.y, model.ptr<float>(i));
}
if (fine_model.empty()) {
N = 1;
fine_model = model;
}
else {
++N;
fine_model = (float(N - 1) / N) * fine_model + (1.0f / N) * model;
}
}
Point3f MT::fast_test(Warp warp)
{
Rect2f rect = window(warp.t);
float fast_stride = sqrt(rect.area() / fast_n);
feature.set_cell(fast_stride);
Rect2f region = window(warp.t / (1.0f + padding));
float minminx = -rect.width * 0.5f;
float minminy = -rect.height * 0.5f;
float maxmaxx = image_size.width + rect.width * 0.5f;
float maxmaxy = image_size.height + rect.height * 0.5f;
int minx = int(round(max(region.x, minminx)));
int miny = int(round(max(region.y, minminy)));
int maxx = int(round(min(region.x + region.width, maxmaxx) - rect.width));
int maxy = int(round(min(region.y + region.height, maxmaxy) - rect.height));
float best_score = 0.0f;
Point3f best_translate = warp.t;
for (int y = miny; y <= maxy; y += fast_step)
for (int x = minx; x <= maxx; x += fast_step) {
float S = 0.0f, score = 0.0f;
for (int i = 0; i < fast_samples.size(); ++i) {
int tx = x + fast_samples[i].x;
int ty = y + fast_samples[i].y;
float *f = fast_model.ptr<float>(i);
float *g = feature.cell_hist(tx, ty);
S += feature.cell_norm(tx, ty);
for (int j = 0; j < L; ++j)
score += f[j] * g[j];
}
score *= S < 1.0f ? 0.0f : 1.0f / sqrt(S);
if (score > best_score) {
best_translate = locate(Rect2f(float(x), float(y), rect.width, rect.height));
best_score = score;
}
}
return best_translate;
}
// static
Rect2f Rect2f::united( const Rect2f& r0, const Rect2f& r1 )
{
assert( r0.isStandard() && r1.isStandard() );
Vector2f r0Min = r0.leftBottom();
Vector2f r0Max = r0.rightTop();
Vector2f r1Min = r1.leftBottom();
Vector2f r1Max = r1.rightTop();
Vector2f unitedMin{ std::min( r0Min.x, r1Min.x ), std::min( r0Min.y, r1Min.y ) };
Vector2f unitedMax{ std::max( r0Max.x, r1Max.x ), std::max( r0Max.y, r1Max.y ) };
return Rect2f( unitedMin, unitedMax - unitedMin );
}
// static
bool Rect2f::intersect( const Rect2f& r0, const Rect2f& r1, Rect2f& intersection )
{
assert( r0.isStandard() && r1.isStandard() );
Vector2f minimum = libcgt::core::math::maximum( r0.minimum(), r1.minimum() );
Vector2f maximum = libcgt::core::math::minimum( r0.maximum(), r1.maximum() );
if( minimum.x < maximum.x &&
minimum.y < maximum.y )
{
intersection.origin = minimum;
intersection.size = maximum - minimum;
return true;
}
return false;
}
MT::MT(Mat img, Rect2f rect, ostream *os) :
log(os),
image_size(img.size()),
window_size(rect.size()),
feature(image_size),
warp(image_size)
{
warp.set(locate(rect));
float fine_stride = sqrt(window_size.area() / fine_n);
int W = int(floor(window_size.width / (2.0f * fine_stride)));
int H = int(floor(window_size.height / (2.0f * fine_stride)));
for (int y = 0; y <= 2 * H; ++y)
for (int x = 0; x <= 2 * W; ++x)
fine_samples.push_back(Point3f((x - W) * fine_stride, (y - H) * fine_stride, 0.0f));
feature.process(img, 0.0f);
fine_train(warp);
fast_train(warp);
error = 0.0f;
roll = yaw = pitch = 0.0f;
count = N = 0;
}