/*******************************************************************
Tracker based on Variable Mean-Shift algorithm and Interpolate Template Matching
Version: MT1
Compiler: Microsoft Visual Studio.net
Luigi Sgaglione
**********************************************************************/
#include <memory>
#include "ObjectTracker.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <iostream>
#include <ios>
#include <algorithm>
using namespace std;
#define MEANSHIFT_ITERATION_NO 15
#define ALPHA 0.98
#define EDGE_DETECT_THRESHOLD 32
#define ITERACTION_THRESHOLD 0.4
#define MAX_FLOAT 3.40282e+38;
//---------------------------------------------------------------------------------------------------------
// Constructor
CObjectTracker::CObjectTracker(int imW, int imH, bool _colorimage,
bool _att_background, bool _man_occlusion) {
att_background = _att_background;
colorimage = _colorimage;
m_nImageWidth = imW;
m_nImageHeight = imH;
track = true;
man_occlusion = _man_occlusion;
m_initialized = false;
for (int i = 0; i < 30; i++) {
neighbours[i].X = -1;
neighbours[i].Y = -1;
neighbours[i].dist2 = MAX_FLOAT;
neighbours[i].X_old = -1;
neighbours[i].Y_old = -1;
}
if (!colorimage)
HISTOGRAM_LENGTH = 512;
else
HISTOGRAM_LENGTH = 8192;
m_sTrackingObject.initHistogram.reset(new float[HISTOGRAM_LENGTH]);
if (att_background)
m_sTrackingObject.weights_background.reset(new float[HISTOGRAM_LENGTH]);
else
m_sTrackingObject.weights_background.reset();
m_sTrackingObject.Status = false;
for (short j = 0; j < HISTOGRAM_LENGTH; j++)
m_sTrackingObject.initHistogram[j] = 0;
};
//--------------------------------------------------------------------------------------------------------
// Destructor
CObjectTracker::~CObjectTracker() {}
//--------------------------------------------------------------------------------------------------------
// Get pixel value RGB
ValuePixel CObjectTracker::GetPixelValues(TRaster32P *frame, short x, short y) {
TPixel32 *data;
ValuePixel pixelValues;
(*frame)->lock();
data = x + (*frame)->pixels(abs(y - m_nImageHeight + 1));
pixelValues.b = data->b;
pixelValues.g = data->g;
pixelValues.r = data->r;
(*frame)->unlock();
return (pixelValues);
}
//--------------------------------------------------------------------------------------------------------
// Set pixel value RGB
void CObjectTracker::SetPixelValues(TRaster32P *frame, ValuePixel pixelValues,
short x, short y) {
TPixel32 *data;
(*frame)->lock();
data = x + (*frame)->pixels(abs(y - m_nImageHeight + 1));
data->b = pixelValues.b;
data->g = pixelValues.g;
data->r = pixelValues.r;
(*frame)->unlock();
}
//--------------------------------------------------------------------------------------------------------
// Inizialization parameters object
void CObjectTracker::ObjectTrackerInitObjectParameters(
short id, short x, short y, short Width, short Height, short _dim,
short _var_dim, float _dist, float _distB) {
objID = id;
m_sTrackingObject.dim_temp = _dim;
m_sTrackingObject.var_dim = _var_dim;
m_sTrackingObject.threshold_distance = _dist;
m_sTrackingObject.threshold_distance_B = _distB;
m_sTrackingObject.X = x;
m_sTrackingObject.Y = y;
m_sTrackingObject.W = Width;
m_sTrackingObject.H = Height;
m_sTrackingObject.X_old = x;
m_sTrackingObject.Y_old = y;
m_sTrackingObject.W_old = Width;
m_sTrackingObject.H_old = Height;
m_sTrackingObject.X_temp = x;
m_sTrackingObject.Y_temp = y;
m_sTrackingObject.W_temp = Width;
m_sTrackingObject.H_temp = Height;
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = 0;
m_sTrackingObject.Status = true;
m_sTrackingObject.assignedAnObject = false;
}
//--------------------------------------------------------------------------------------------------------
// Update frame template
void CObjectTracker::updateTemp() {
m_sTrackingObject.X_temp = m_sTrackingObject.X;
m_sTrackingObject.Y_temp = m_sTrackingObject.Y;
m_sTrackingObject.W_temp = m_sTrackingObject.W;
m_sTrackingObject.H_temp = m_sTrackingObject.H;
}
//--------------------------------------------------------------------------------------------------------
// Object Tracking
void CObjectTracker::ObjeckTrackerHandlerByUser(TRaster32P *frame) {
if (m_sTrackingObject.Status) {
if (!m_sTrackingObject.assignedAnObject) {
FindHistogram(frame, m_sTrackingObject.initHistogram.get(), 1);
m_sTrackingObject.assignedAnObject = true;
/*
ofstream output;
output.open("output.txt",ios_base::app);
output<<m_sTrackingObject.X <<" "<<m_sTrackingObject.Y<<endl;
output<<m_sTrackingObject.W <<" "<<m_sTrackingObject.H<<endl;
output<<m_sTrackingObject.half_pixelx <<"
"<<m_sTrackingObject.half_pixely<<endl;
if (m_visible=="")
{
output<<"VISIBLE"<<endl;
}
else
{
output<<m_visible<<endl;
}
output.close();
output.open("output_center.txt",ios_base::app);
output<<"Coordinate del punto - X : "<<m_sTrackingObject.X -
((m_nImageWidth - 1)/2)<<" - Y : "<<m_sTrackingObject.Y - ((m_nImageHeight
- 1)/2)<<endl;
output<<"Larghezza dell'area : "<<m_sTrackingObject.W <<" - Altezza
dell'aera : "<<m_sTrackingObject.H<<endl;
output<<"Mezzo pixel : "<<m_sTrackingObject.half_pixelx <<"
"<<m_sTrackingObject.half_pixely<<endl;
if (m_visible=="")
{
output<<"Visibilità : VISIBLE"<<endl;
}
else
{
output<<"Visibilità : "<<m_visible<<endl;
}
output.close();*/
} else {
FindNextLocation(frame);
}
}
}
//--------------------------------------------------------------------------------------------------------
// histogram object
void CObjectTracker::FindHistogram(TRaster32P *frame, float(*histogram),
float h) {
short normx = 0, normy = 0;
float normc = 0.0, normc1 = 0.0;
short i = 0;
short x = 0;
short y = 0;
UBYTE8 E = 0;
UBYTE8 qR = 0, qG = 0, qB = 0;
ValuePixel pixelValues;
int indice = 0;
for (i = 0; i < HISTOGRAM_LENGTH; i++) histogram[i] = 0.0;
if ((colorimage) && (att_background)) FindWeightsBackground(frame);
// normalization
normx = short(m_sTrackingObject.X + m_sTrackingObject.W / 2);
normy = short(m_sTrackingObject.Y + m_sTrackingObject.H / 2);
for (y = std::max(m_sTrackingObject.Y - m_sTrackingObject.H / 2, 0);
y <= std::min(m_sTrackingObject.Y + m_sTrackingObject.H / 2,
m_nImageHeight - 1);
y++)
for (x = std::max(m_sTrackingObject.X - m_sTrackingObject.W / 2, 0);
x <= std::min(m_sTrackingObject.X + m_sTrackingObject.W / 2,
m_nImageWidth - 1);
x++) {
E = CheckEdgeExistence(frame, x, y);
pixelValues = GetPixelValues(frame, x, y);
// components RGB "quantizzate"
if (!colorimage) {
indice = 256 * E + pixelValues.r;
} else {
qR = pixelValues.r / 16;
qG = pixelValues.g / 16;
qB = pixelValues.b / 16;
indice = 4096 * E + 256 * qR + 16 * qG + qB;
}
histogram[indice] += 1 -
(((m_sTrackingObject.X - x) / normx) *
((m_sTrackingObject.X - x) / normx) +
((m_sTrackingObject.Y - y) / normy) *
((m_sTrackingObject.Y - y) / normy)) /
(h * h);
}
if ((colorimage) && (att_background)) {
for (i = 0; i < HISTOGRAM_LENGTH; i++)
histogram[i] *= m_sTrackingObject.weights_background[i];
// normalization pdf
for (i = 0; i < HISTOGRAM_LENGTH; i++) {
normc += histogram[i];
normc1 += m_sTrackingObject.weights_background[i];
}
// Pdf
for (i = 0; i < HISTOGRAM_LENGTH; i++)
histogram[i] = histogram[i] / (normc * normc1);
}
else {
for (i = 0; i < HISTOGRAM_LENGTH; i++) normc += histogram[i];
for (i = 0; i < HISTOGRAM_LENGTH; i++)
histogram[i] = histogram[i] / (normc);
}
}
//--------------------------------------------------------------------------------------------------------
// Histogram background
void CObjectTracker::FindHistogramBackground(TRaster32P *frame,
float(*background)) {
short i = 0;
short x = 0;
short y = 0;
UBYTE8 E = 0;
UBYTE8 qR = 0, qG = 0, qB = 0;
ValuePixel pixelValues;
UINT32 pix = 0;
for (i = 0; i < HISTOGRAM_LENGTH; i++) background[i] = 0.0;
for (y = std::max(m_sTrackingObject.Y - (m_sTrackingObject.H * 1.73) / 2,
0.0);
y <= std::min(m_sTrackingObject.Y + (m_sTrackingObject.H * 1.73) / 2,
m_nImageHeight - 1.0);
y++)
for (x = std::max(m_sTrackingObject.X - (m_sTrackingObject.W * 1.73) / 2,
0.0);
x <= std::min(m_sTrackingObject.X + (m_sTrackingObject.W * 1.73) / 2,
m_nImageWidth - 1.0);
x++) {
if (((m_sTrackingObject.Y - m_sTrackingObject.H / 2) <= y) &&
(y <= (m_sTrackingObject.Y + m_sTrackingObject.H / 2)) &&
((m_sTrackingObject.X - m_sTrackingObject.W / 2) <= x) &&
(x <= (m_sTrackingObject.X + m_sTrackingObject.W / 2)))
continue;
E = CheckEdgeExistence(frame, x, y);
pixelValues = GetPixelValues(frame, x, y);
// components RGB "quantizzate"
qR = pixelValues.r / 16;
qG = pixelValues.g / 16;
qB = pixelValues.b / 16;
background[4096 * E + 256 * qR + 16 * qG + qB] += 1;
pix++;
}
// Pdf
for (i = 0; i < HISTOGRAM_LENGTH; i++) background[i] = background[i] / pix;
}
//--------------------------------------------------------------------------------------------------------
// Weights Background
void CObjectTracker::FindWeightsBackground(TRaster32P *frame) {
float small1;
std::unique_ptr<float[]> background(new float[HISTOGRAM_LENGTH]);
short i;
for (i = 0; i < HISTOGRAM_LENGTH; i++)
m_sTrackingObject.weights_background[i] = 0.0;
// Histogram background
FindHistogramBackground(frame, background.get());
// searce min != 0.0
for (i = 0; background[i] == 0.0; i++)
;
small1 = background[i];
for (i = 0; i < HISTOGRAM_LENGTH; i++)
if ((background[i] != 0.0) && (background[i] < small1))
small1 = background[i];
// weights
for (i = 0; i < HISTOGRAM_LENGTH; i++) {
if (background[i] == 0.0)
m_sTrackingObject.weights_background[i] = 1;
else
m_sTrackingObject.weights_background[i] = small1 / background[i];
}
}
//--------------------------------------------------------------------------------------------------------
// new location
void CObjectTracker::FindWightsAndCOM(TRaster32P *frame, float(*histogram)) {
short i = 0;
short x = 0;
short y = 0;
UBYTE8 E = 0;
float sumOfWeights = 0;
short ptr = 0;
UBYTE8 qR = 0, qG = 0, qB = 0;
float newX = 0.0;
float newY = 0.0;
ValuePixel pixelValues;
std::unique_ptr<float[]> weights(new float[HISTOGRAM_LENGTH]);
// weights
for (i = 0; i < HISTOGRAM_LENGTH; i++) {
if (histogram[i] > 0.0)
weights[i] = sqrt(m_sTrackingObject.initHistogram[i] / histogram[i]);
else
weights[i] = 0.0;
}
// new location
for (y = std::max(m_sTrackingObject.Y - m_sTrackingObject.H / 2, 0);
y <= std::min(m_sTrackingObject.Y + m_sTrackingObject.H / 2,
m_nImageHeight - 1);
y++)
for (x = std::max(m_sTrackingObject.X - m_sTrackingObject.W / 2, 0);
x <= std::min(m_sTrackingObject.X + m_sTrackingObject.W / 2,
m_nImageWidth - 1);
x++) {
E = CheckEdgeExistence(frame, x, y);
pixelValues = GetPixelValues(frame, x, y);
if (!colorimage) {
ptr = 256 * E + pixelValues.r;
} else {
qR = pixelValues.r / 16;
qG = pixelValues.g / 16;
qB = pixelValues.b / 16;
ptr = 4096 * E + 256 * qR + 16 * qG + qB;
}
newX += (weights[ptr] * x);
newY += (weights[ptr] * y);
sumOfWeights += weights[ptr];
}
if (sumOfWeights > 0) {
m_sTrackingObject.X = short((newX / sumOfWeights) + 0.5);
m_sTrackingObject.Y = short((newY / sumOfWeights) + 0.5);
}
}
//--------------------------------------------------------------------------------------------------------
// Edge Information of pixel
UBYTE8 CObjectTracker::CheckEdgeExistence(TRaster32P *frame, short _x,
short _y) {
UBYTE8 E = 0;
short GrayCenter = 0;
short GrayLeft = 0;
short GrayRight = 0;
short GrayUp = 0;
short GrayDown = 0;
ValuePixel pixelValues;
pixelValues = GetPixelValues(frame, _x, _y);
GrayCenter = short(3 * pixelValues.r + 6 * pixelValues.g + pixelValues.b);
if (_x > 0) {
pixelValues = GetPixelValues(frame, _x - 1, _y);
GrayLeft = short(3 * pixelValues.r + 6 * pixelValues.g + pixelValues.b);
}
if (_x < (m_nImageWidth - 1)) {
pixelValues = GetPixelValues(frame, _x + 1, _y);
GrayRight = short(3 * pixelValues.r + 6 * pixelValues.g + pixelValues.b);
}
if (_y > 0) {
pixelValues = GetPixelValues(frame, _x, _y - 1);
GrayUp = short(3 * pixelValues.r + 6 * pixelValues.g + pixelValues.b);
}
if (_y < (m_nImageHeight - 1)) {
pixelValues = GetPixelValues(frame, _x, _y + 1);
GrayDown = short(3 * pixelValues.r + 6 * pixelValues.g + pixelValues.b);
}
if (abs((GrayCenter - GrayLeft) / 10) > EDGE_DETECT_THRESHOLD) E = 1;
if (abs((GrayCenter - GrayRight) / 10) > EDGE_DETECT_THRESHOLD) E = 1;
if (abs((GrayCenter - GrayUp) / 10) > EDGE_DETECT_THRESHOLD) E = 1;
if (abs((GrayCenter - GrayDown) / 10) > EDGE_DETECT_THRESHOLD) E = 1;
return (E);
}
//--------------------------------------------------------------------------------------------------------
// Mean-shift
void CObjectTracker::FindNextLocation(TRaster32P *frame) {
short i = 0;
short iteration = 0;
short xold = 0;
short yold = 0;
float distanza;
float Height, Width;
float h = 1.0;
float h_opt;
float DELTA;
double rho, rho1, rho2;
std::unique_ptr<float[]> currentHistogram(new float[HISTOGRAM_LENGTH]);
Height = m_sTrackingObject.H;
Width = m_sTrackingObject.W;
// old
m_sTrackingObject.W_old = m_sTrackingObject.W;
m_sTrackingObject.H_old = m_sTrackingObject.H;
m_sTrackingObject.X_old = m_sTrackingObject.X;
m_sTrackingObject.Y_old = m_sTrackingObject.Y;
for (iteration = 0; iteration < MEANSHIFT_ITERATION_NO; iteration++) {
rho = 0.0;
rho1 = 0.0;
rho2 = 0.0;
DELTA = 0.1 * h;
// Prew center
xold = m_sTrackingObject.X;
yold = m_sTrackingObject.Y;
// Histogram with bandwidth h
FindHistogram(frame, currentHistogram.get(), h);
// New location
FindWightsAndCOM(frame, currentHistogram.get());
// Histogram with new location
FindHistogram(frame, currentHistogram.get(), h);
// Battacharyya coefficient
for (i = 0; i < HISTOGRAM_LENGTH; i++)
rho += sqrt(m_sTrackingObject.initHistogram[i] * currentHistogram[i]);
// old center
m_sTrackingObject.X = xold;
m_sTrackingObject.Y = yold;
// Histogram with bandwidth h-DELTA
m_sTrackingObject.H = Height - m_sTrackingObject.var_dim;
m_sTrackingObject.W = Width - m_sTrackingObject.var_dim;
FindHistogram(frame, currentHistogram.get(), h - DELTA);
// New location
FindWightsAndCOM(frame, currentHistogram.get());
// Histogram with new location
FindHistogram(frame, currentHistogram.get(), h - DELTA);
// Battacharyya coefficient
for (i = 0; i < HISTOGRAM_LENGTH; i++)
rho1 += sqrt(m_sTrackingObject.initHistogram[i] * currentHistogram[i]);
// old center
m_sTrackingObject.X = xold;
m_sTrackingObject.Y = yold;
// Histogram with bandwidth h+DELTA
m_sTrackingObject.H = Height + m_sTrackingObject.var_dim;
m_sTrackingObject.W = Width + m_sTrackingObject.var_dim;
FindHistogram(frame, currentHistogram.get(), h + DELTA);
// New location
FindWightsAndCOM(frame, currentHistogram.get());
// Histogram with new location
FindHistogram(frame, currentHistogram.get(), h + DELTA);
// Battacharyya coefficient
for (i = 0; i < HISTOGRAM_LENGTH; i++)
rho2 += sqrt(m_sTrackingObject.initHistogram[i] * currentHistogram[i]);
// h_optimal
if ((rho >= rho1) && (rho >= rho2))
h_opt = h;
else if (rho1 > rho2) {
h_opt = h - DELTA;
Height -= m_sTrackingObject.var_dim;
Width -= m_sTrackingObject.var_dim;
} else {
h_opt = h + DELTA;
Height += m_sTrackingObject.var_dim;
Width += m_sTrackingObject.var_dim;
}
// oversensitive scale adaptation
h = 0.1 * h_opt + (1 - 0.1) * h;
// New dimension Object box
m_sTrackingObject.H = Height;
m_sTrackingObject.W = Width;
// old center
m_sTrackingObject.X = xold;
m_sTrackingObject.Y = yold;
// Current Histogram
FindHistogram(frame, currentHistogram.get(), h);
// Definitive new location
FindWightsAndCOM(frame, currentHistogram.get());
// threshold
distanza = sqrt(
float((xold - m_sTrackingObject.X) * (xold - m_sTrackingObject.X) +
(yold - m_sTrackingObject.Y) * (yold - m_sTrackingObject.Y)));
if (distanza <= ITERACTION_THRESHOLD) break;
}
// New Histogram
FindHistogram(frame, currentHistogram.get(), h);
// Update
UpdateInitialHistogram(currentHistogram.get());
}
//--------------------------------------------------------------------------------------------------------
// Update Initial histogram
void CObjectTracker::UpdateInitialHistogram(float(*histogram)) {
short i = 0;
for (i = 0; i < HISTOGRAM_LENGTH; i++)
m_sTrackingObject.initHistogram[i] =
ALPHA * m_sTrackingObject.initHistogram[i] + (1 - ALPHA) * histogram[i];
}
//-------------------------------------------------------------------------------------------------------
// Template Matching
float CObjectTracker::Matching(TRaster32P *frame, TRaster32P *frame_temp) {
float dist = 0.0;
float min_dist = MAX_FLOAT;
short u, v, x, y;
short u_sup, v_sup;
short x_min, y_min;
short x_max, y_max;
short dimx, dimy;
short dimx_int, dimy_int;
short ok_u = 0;
short ok_v = 0;
if (!track) {
if (m_sTrackingObject.X != -1) {
m_sTrackingObject.X_old = m_sTrackingObject.X;
m_sTrackingObject.Y_old = m_sTrackingObject.Y;
} else {
m_sTrackingObject.X = m_sTrackingObject.X_old;
m_sTrackingObject.Y = m_sTrackingObject.Y_old;
}
}
std::unique_ptr<short[]> u_att(new short[2 * m_sTrackingObject.dim_temp + 1]);
std::unique_ptr<short[]> v_att(new short[2 * m_sTrackingObject.dim_temp + 1]);
x_min = std::max(m_sTrackingObject.X_temp - m_sTrackingObject.W_temp / 2, 0);
y_min = std::max(m_sTrackingObject.Y_temp - m_sTrackingObject.H_temp / 2, 0);
x_max = std::min(m_sTrackingObject.X_temp + m_sTrackingObject.W_temp / 2,
m_nImageWidth - 1);
y_max = std::min(m_sTrackingObject.Y_temp + m_sTrackingObject.H_temp / 2,
m_nImageHeight - 1);
// dimension template
dimx = x_max - x_min + 1;
dimy = y_max - y_min + 1;
// dimension interpolate template
dimx_int = dimx * 2 - 1;
dimy_int = dimy * 2 - 1;
// searce area
int in = 0;
for (u = -m_sTrackingObject.dim_temp; u <= m_sTrackingObject.dim_temp; u++) {
if (((m_sTrackingObject.X + u - dimx / 2) < 0) ||
((m_sTrackingObject.X + u + dimx / 2) > m_nImageWidth - 1))
u_att[in] = 0;
else {
u_att[in] = 1;
ok_u += 1;
}
in++;
}
in = 0;
for (v = -m_sTrackingObject.dim_temp; v <= m_sTrackingObject.dim_temp; v++) {
if (((m_sTrackingObject.Y + v - dimy / 2) < 0) ||
((m_sTrackingObject.Y + v + dimy / 2) > m_nImageHeight - 1))
v_att[in] = 0;
else {
v_att[in] = 1;
ok_v += 1;
}
in++;
}
if ((ok_u > 0) && (ok_v > 0)) {
// Interpolate template
std::unique_ptr<ValuePixel[]> pixel_temp(
new ValuePixel[dimx_int * dimy_int]);
// original value
for (int i = 0; i <= (dimx - 1); i++)
for (int j = 0; j <= (dimy - 1); j++) {
pixel_temp[i * dimy_int * 2 + j * 2] =
GetPixelValues(frame_temp, x_min + i, y_min + j);
}
// Interpolate value row
for (int i = 1; i < (dimx_int * dimy_int); i += 2) {
pixel_temp[i].r = (pixel_temp[i - 1].r + pixel_temp[i + 1].r) / 2;
pixel_temp[i].g = (pixel_temp[i - 1].g + pixel_temp[i + 1].g) / 2;
pixel_temp[i].b = (pixel_temp[i - 1].b + pixel_temp[i + 1].b) / 2;
if (i % dimy_int == dimy_int - 2) i += dimy_int + 1;
}
// Interpolate value column
for (int i = dimy_int; i <= ((dimx_int - 1) * dimy_int - 1); i += 2) {
pixel_temp[i].r =
(pixel_temp[i - dimy_int].r + pixel_temp[i + dimy_int].r) / 2;
pixel_temp[i].g =
(pixel_temp[i - dimy_int].g + pixel_temp[i + dimy_int].g) / 2;
pixel_temp[i].b =
(pixel_temp[i - dimy_int].b + pixel_temp[i + dimy_int].b) / 2;
if (i % dimy_int == dimy_int - 1) i += dimy_int - 1;
}
// Interpolate value central
for (int i = dimy_int + 1; i <= ((dimx_int - 1) * dimy_int - 2); i += 2) {
pixel_temp[i].r = UBYTE8(float((pixel_temp[i - dimy_int - 1].r +
pixel_temp[i - dimy_int + 1].r +
pixel_temp[i + dimy_int - 1].r +
pixel_temp[i + dimy_int + 1].r) /
4) +
0.4);
pixel_temp[i].g = UBYTE8(float((pixel_temp[i - dimy_int - 1].g +
pixel_temp[i - dimy_int + 1].g +
pixel_temp[i + dimy_int - 1].g +
pixel_temp[i + dimy_int + 1].g) /
4) +
0.4);
pixel_temp[i].b = UBYTE8(float((pixel_temp[i - dimy_int - 1].b +
pixel_temp[i - dimy_int + 1].b +
pixel_temp[i + dimy_int - 1].b +
pixel_temp[i + dimy_int + 1].b) /
4) +
0.4);
if (i % dimy_int == dimy_int - 2) i += dimy_int + 1;
}
//--------------------------------------------------------
short dimx_int_ric, dimy_int_ric;
short x_min_ric, y_min_ric;
short u_min = -m_sTrackingObject.dim_temp;
short v_min = -m_sTrackingObject.dim_temp;
for (int i = 0; i <= 2 * m_sTrackingObject.dim_temp; i++)
if (u_att[i] == 1)
break;
else
u_min++;
x_min_ric = m_sTrackingObject.X + u_min - dimx / 2;
for (int i = 0; i <= 2 * m_sTrackingObject.dim_temp; i++)
if (v_att[i] == 1)
break;
else
v_min++;
y_min_ric = m_sTrackingObject.Y + v_min - dimy / 2;
// Effective Dimension searce interpolate area
dimx_int_ric = ((dimx + ok_u - 1) * 2 - 1);
dimy_int_ric = ((dimy + ok_v - 1) * 2 - 1);
std::unique_ptr<ValuePixel[]> area_ricerca(
new ValuePixel[dimx_int_ric * dimy_int_ric]);
// Original value
for (int i = 0; i <= ((dimx + ok_u - 1) - 1); i++)
for (int j = 0; j <= ((dimy + ok_v - 1) - 1); j++) {
area_ricerca[i * dimy_int_ric * 2 + j * 2] =
GetPixelValues(frame, x_min_ric + i, y_min_ric + j);
}
// interpolate value row
for (int i = 1; i < (dimx_int_ric * dimy_int_ric); i += 2) {
area_ricerca[i].r = (area_ricerca[i - 1].r + area_ricerca[i + 1].r) / 2;
area_ricerca[i].g = (area_ricerca[i - 1].g + area_ricerca[i + 1].g) / 2;
area_ricerca[i].b = (area_ricerca[i - 1].b + area_ricerca[i + 1].b) / 2;
if (i % dimy_int_ric == dimy_int_ric - 2) i += dimy_int_ric + 1;
}
// Interpolate value column
for (int i = dimy_int_ric; i <= ((dimx_int_ric - 1) * dimy_int_ric - 1);
i += 2) {
area_ricerca[i].r = (area_ricerca[i - dimy_int_ric].r +
area_ricerca[i + dimy_int_ric].r) /
2;
area_ricerca[i].g = (area_ricerca[i - dimy_int_ric].g +
area_ricerca[i + dimy_int_ric].g) /
2;
area_ricerca[i].b = (area_ricerca[i - dimy_int_ric].b +
area_ricerca[i + dimy_int_ric].b) /
2;
if (i % dimy_int_ric == dimy_int_ric - 1) i += dimy_int_ric - 1;
}
// Interpolate value central
for (int i = dimy_int_ric + 1; i <= ((dimx_int_ric - 1) * dimy_int_ric - 2);
i += 2) {
area_ricerca[i].r = UBYTE8(float((area_ricerca[i - dimy_int_ric - 1].r +
area_ricerca[i - dimy_int_ric + 1].r +
area_ricerca[i + dimy_int_ric - 1].r +
area_ricerca[i + dimy_int_ric + 1].r) /
4) +
0.4);
area_ricerca[i].g = UBYTE8(float((area_ricerca[i - dimy_int_ric - 1].g +
area_ricerca[i - dimy_int_ric + 1].g +
area_ricerca[i + dimy_int_ric - 1].g +
area_ricerca[i + dimy_int_ric + 1].g) /
4) +
0.4);
area_ricerca[i].b = UBYTE8(float((area_ricerca[i - dimy_int_ric - 1].b +
area_ricerca[i - dimy_int_ric + 1].b +
area_ricerca[i + dimy_int_ric - 1].b +
area_ricerca[i + dimy_int_ric + 1].b) /
4) +
0.4);
if (i % dimy_int_ric == dimy_int_ric - 2) i += dimy_int_ric + 1;
}
short u_max = m_sTrackingObject.dim_temp;
short v_max = m_sTrackingObject.dim_temp;
for (int i = 2 * m_sTrackingObject.dim_temp; i >= 0; i--)
if (u_att[i] == 1)
break;
else
u_max--;
for (int i = 2 * m_sTrackingObject.dim_temp; i >= 0; i--)
if (v_att[i] == 1)
break;
else
v_max--;
unsigned long indt, indc;
std::unique_ptr<float[]> mat_dist(
new float[(2 * ok_u - 1) * (2 * ok_v - 1)]);
float att_dist_cent = MAX_FLOAT;
float dist_cent;
// Distance
for (u = 2 * u_min; u <= 2 * u_max; u++) {
for (v = 2 * v_min; v <= 2 * v_max; v++) {
dist = 0.0;
//
for (x = 0; x < dimx_int; x++)
for (y = 0; y < dimy_int; y++) {
// T(x,y)
indt = x * dimy_int + y;
// L(x+u,y+v)
indc =
((x + (u - 2 * u_min))) * dimy_int_ric + (y + (v - 2 * v_min));
dist += (pixel_temp[indt].r - area_ricerca[indc].r) *
(pixel_temp[indt].r - area_ricerca[indc].r) +
(pixel_temp[indt].g - area_ricerca[indc].g) *
(pixel_temp[indt].g - area_ricerca[indc].g) +
(pixel_temp[indt].b - area_ricerca[indc].b) *
(pixel_temp[indt].b - area_ricerca[indc].b);
}
dist = sqrt(dist) / (sqrt(float(dimx_int * dimy_int * 3 * 255 * 255)));
mat_dist[(u - 2 * u_min) * (2 * ok_v - 1) + (v - 2 * v_min)] = dist;
if (dist < min_dist) {
min_dist = dist;
u_sup = u;
v_sup = v;
att_dist_cent = sqrt(float(u * u + v * v));
}
else if (dist == min_dist) {
dist_cent = sqrt(float(u * u + v * v));
if (dist_cent < att_dist_cent) {
min_dist = dist;
u_sup = u;
v_sup = v;
att_dist_cent = dist_cent;
}
}
}
}
if (!man_occlusion) {
if (min_dist > m_sTrackingObject.threshold_distance) {
track = false;
m_sTrackingObject.X = m_sTrackingObject.X_old;
m_sTrackingObject.Y = m_sTrackingObject.Y_old;
m_visible = "INVISIBLE";
} else {
track = true;
if (min_dist > m_sTrackingObject.threshold_distance_B) {
m_visible = "WARNING";
m_K_dist = floor(
(double)(min_dist - m_sTrackingObject.threshold_distance_B) /
(m_sTrackingObject.threshold_distance -
m_sTrackingObject.threshold_distance_B));
} else {
m_visible = "VISIBLE";
}
}
}
if (track) {
// half pixel
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = 0;
if (u_sup % 2 != 0)
if (v_sup % 2 != 0) {
if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)])
if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)])
if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)]) {
m_sTrackingObject.half_pixelx = -1;
m_sTrackingObject.half_pixely = -1;
u_sup -= 1;
v_sup -= 1;
} else {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = 1;
u_sup += 1;
v_sup += 1;
}
else if (mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)]) {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = -1;
u_sup += 1;
v_sup -= 1;
} else {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = 1;
u_sup += 1;
v_sup += 1;
}
else if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)])
if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)]) {
m_sTrackingObject.half_pixelx = -1;
m_sTrackingObject.half_pixely = 1;
u_sup -= 1;
v_sup += 1;
} else {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = 1;
u_sup += 1;
v_sup += 1;
}
else if (mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)]) {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = -1;
u_sup += 1;
v_sup -= 1;
} else {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = 1;
u_sup += 1;
v_sup += 1;
}
} else {
if (mat_dist[(u_sup - 2 * u_min - 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min)] <
mat_dist[(u_sup - 2 * u_min + 1) * (2 * ok_v - 1) +
(v_sup - 2 * v_min)]) {
m_sTrackingObject.half_pixelx = -1;
m_sTrackingObject.half_pixely = 0;
u_sup -= 1;
} else {
m_sTrackingObject.half_pixelx = 1;
m_sTrackingObject.half_pixely = 0;
u_sup += 1;
}
}
else if (v_sup % 2 != 0) {
if (mat_dist[(u_sup - 2 * u_min) * (2 * ok_v - 1) +
(v_sup - 2 * v_min - 1)] <
mat_dist[(u_sup - 2 * u_min) * (2 * ok_v - 1) +
(v_sup - 2 * v_min + 1)]) {
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = -1;
v_sup -= 1;
} else {
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = 1;
v_sup += 1;
}
}
m_sTrackingObject.X += u_sup / 2;
m_sTrackingObject.Y += v_sup / 2;
}
}
return min_dist;
}
//-------------------------------------------------------------------------------------------------------
// Update characteristic neighbours
void CObjectTracker::DistanceAndUpdate(NEIGHBOUR position) {
float x0 = m_sTrackingObject.X;
float y0 = m_sTrackingObject.Y;
float x1 = position.X_old, y1 = position.Y_old;
float dist2;
dist2 = (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
// searce free position
for (int i = 0; i < 30; i++) {
if ((neighbours[i].X == -1) && (neighbours[i].Y == -1)) {
neighbours[i].X = position.X;
neighbours[i].Y = position.Y;
neighbours[i].dist2 = dist2;
neighbours[i].X_old = position.X_old;
neighbours[i].Y_old = position.Y_old;
return;
}
}
// sostitution based on distance
int i = 0;
for (i = 0; i < 30; i++) {
if (dist2 < neighbours[i].dist2) {
neighbours[i].X = position.X;
neighbours[i].Y = position.Y;
neighbours[i].dist2 = dist2;
neighbours[i].X_old = position.X_old;
neighbours[i].Y_old = position.Y_old;
return;
}
}
}
//--------------------------------------------------------------------------------------------------------
// Set position by neighbours
void CObjectTracker::SetPositionByNeighbours(void) {
if (!track) {
float x[3], y[3], d2[3];
for (int i = 0; i < 30; i++) {
//((X,Y)=(-1,-1) => neighbours not valid)
if ((neighbours[i].X == -1) && (neighbours[i].Y == -1)) {
m_sTrackingObject.X = -1;
m_sTrackingObject.Y = -1;
return;
} else {
x[i] = neighbours[i].X;
y[i] = neighbours[i].Y;
d2[i] = neighbours[i].dist2;
}
}
// error: neighbours aligns
if ((y[1] - y[0]) * (x[2] - x[0]) == (y[2] - y[0]) * (x[1] - x[0])) {
m_sTrackingObject.X = -1;
m_sTrackingObject.Y = -1;
return;
}
// old neighbours coordinate
float x_old[3], y_old[3];
int i = 0;
for (i = 0; i < 3; i++) {
x_old[i] = neighbours[i].X_old;
y_old[i] = neighbours[i].Y_old;
}
//
float dn2[3], dn_old2[3];
// distance
for (i = 0; i < 2; i++)
for (int j = i + 1; j < 3; j++) {
dn_old2[i + j - 1] = (x_old[i] - x_old[j]) * (x_old[i] - x_old[j]) +
(y_old[i] - y_old[j]) * (y_old[i] - y_old[j]);
dn2[i + j - 1] =
(x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
}
// scale factor (^2)
float scale = ((dn2[0] / dn_old2[0]) + (dn2[1] / dn_old2[1]) +
(dn2[2] / dn_old2[2])) /
3.0;
// update distance
for (i = 0; i < 3; i++) d2[i] *= scale;
// new location
float A[2][2], b[2];
A[0][0] = 2 * (x[1] - x[0]);
A[0][1] = 2 * (y[1] - y[0]);
A[1][0] = 2 * (x[2] - x[0]);
A[1][1] = 2 * (y[2] - y[0]);
b[0] =
d2[0] - d2[1] + x[1] * x[1] - x[0] * x[0] + y[1] * y[1] - y[0] * y[0];
b[1] =
d2[0] - d2[2] + x[2] * x[2] - x[0] * x[0] + y[2] * y[2] - y[0] * y[0];
float detA, invA[2][2];
detA = A[0][0] * A[1][1] - A[0][1] * A[1][0];
invA[0][0] = A[1][1] / detA;
invA[0][1] = -A[0][1] / detA;
invA[1][0] = -A[1][0] / detA;
invA[1][1] = A[0][0] / detA;
float X, Y;
X = invA[0][0] * b[0] + invA[0][1] * b[1];
Y = invA[1][0] * b[0] + invA[1][1] * b[1];
m_sTrackingObject.X = short(X + 0.49999);
m_sTrackingObject.Y = short(Y + 0.49999);
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = 0;
// error
if (m_sTrackingObject.X < 0) {
// m_sTrackingObject.X = 0;
m_visible = "INVISIBLE";
}
if (m_sTrackingObject.X > (m_nImageWidth - 1)) {
// m_sTrackingObject.X = m_nImageWidth - 1;
m_visible = "INVISIBLE";
}
if (m_sTrackingObject.Y < 0) {
// m_sTrackingObject.Y = 0;
m_visible = "INVISIBLE";
}
if (m_sTrackingObject.Y > (m_nImageHeight - 1)) {
// m_sTrackingObject.Y = m_nImageHeight - 1;
m_visible = "INVISIBLE";
}
}
}
//--------------------------------------------------------------------------------------------------------
// Write on output file
void CObjectTracker::WriteOnOutputFile(char *filename) {
ofstream output;
output.open(filename, ios_base::app);
output << m_sTrackingObject.X << " " << m_sTrackingObject.Y << endl;
output << m_sTrackingObject.W << " " << m_sTrackingObject.H << endl;
output << m_sTrackingObject.half_pixelx << " "
<< m_sTrackingObject.half_pixely << endl;
output << m_visible << endl;
output.close();
output.open("output_center.txt", ios_base::app);
output << "Coordinate del punto - X : "
<< m_sTrackingObject.X - ((m_nImageWidth - 1) / 2)
<< " - Y : " << m_sTrackingObject.Y - ((m_nImageHeight - 1) / 2)
<< endl;
output << "Larghezza dell'area : " << m_sTrackingObject.W
<< " - Altezza dell'aera : " << m_sTrackingObject.H << endl;
output << "Mezzo pixel : " << m_sTrackingObject.half_pixelx << " "
<< m_sTrackingObject.half_pixely << endl;
output << "Visibilità : " << m_visible << endl;
output.close();
}
//--------------------------------------------------------------------------------------------------------
// Reset distance
void CObjectTracker::DistanceReset(void) {
for (int i = 0; i < 30; i++) {
neighbours[i].X = -1;
neighbours[i].Y = -1;
neighbours[i].dist2 = MAX_FLOAT;
}
}
//--------------------------------------------------------------------------------------------------------
// Get position neighbours
NEIGHBOUR CObjectTracker::GetPosition(void) {
NEIGHBOUR _neighbour;
_neighbour.X = m_sTrackingObject.X;
_neighbour.Y = m_sTrackingObject.Y;
_neighbour.dist2 = MAX_FLOAT;
_neighbour.X_old = m_sTrackingObject.X_old;
_neighbour.Y_old = m_sTrackingObject.Y_old;
return _neighbour;
}
// Set position of the object
void CObjectTracker::SetPosition(short x, short y) {
if (x < 0) {
// x = 0;
m_visible = "INVISIBLE";
} else if (x > (m_nImageWidth - 1)) {
// x = m_nImageWidth - 1;
m_visible = "INVISIBLE";
}
if (y < 0) {
// y = 0;
m_visible = "INVISIBLE";
} else if (y > (m_nImageHeight - 1)) {
// y = m_nImageHeight - 1;
m_visible = "INVISIBLE";
}
m_sTrackingObject.X = x;
m_sTrackingObject.Y = y;
m_sTrackingObject.half_pixelx = 0;
m_sTrackingObject.half_pixely = 0;
}
// Set the object as initialized
void CObjectTracker::SetInit(bool status) { m_initialized = status; }
// Get if the object is initialized
bool CObjectTracker::GetInit() { return m_initialized; }
std::string CObjectTracker::GetVisibility() { return m_visible; }
void CObjectTracker::SetVisibility(string visibility) {
m_visible = visibility;
}
// Set initials position of neighbours
void CObjectTracker::SetInitials(NEIGHBOUR position) {
initial.x = position.X;
initial.y = position.Y;
}
// Get initials position of neighbours
Predict3D::Point CObjectTracker::GetInitials() { return initial; }
// Get the K_Dist
int CObjectTracker::GetKDist() { return m_K_dist; }
// End