void ParticleFilter::measurement(Mat_<double>& particles, Rect detection_new, Mat HSVinterpolated,vector<Mat>& MultiHSVInterpolated, Mat& image, int use_hist, int frame){
//! Measurement step of particle filter
/*!
*/
int x;
int y;
if (rely_detection) { // when no occlusion
x = (int)(detection_new.x + detection_new.width/2);
y = (int)(detection_new.y + detection_new.height/2);
}
else{ // when there is an occlusion
x = PFTrack.x + PFTrackVel.x;
y = PFTrack.y + PFTrackVel.y;
}
//detection term
double w_det;
vector<double> w_detm_temp(numParticles,0.0);
vector<double> w_detm(numParticles,0.0);
double w_sum = 0.0;
for (unsigned int index = 0; index < numParticles; index++) {
double x_term = pow(double(particles_new[index][X_POS]-x), 2)/(2*pow(sigma_measurement,2));
double y_term = pow(double(particles_new[index][Y_POS]-y), 2)/(2*pow(sigma_measurement,2));
w_det = (1/sqrt(2*PI*pow(sigma_measurement,2)))*exp(-1*(x_term+y_term));
w_det = w_det + 1e-99;
// w_sum += w_det;
w_detm_temp[index]=w_det;
}
// normalize weight
for (unsigned int index = 0; index < numParticles; index++) {
w_sum += w_detm_temp[index];
}
for (unsigned int index = 0; index < numParticles; index++) {
w_detm[index] = w_detm_temp[index]/w_sum;
}
//appearance term
double w_col;
w_sum = 0.0;
vector<double> w_colm(numParticles,0.0);
vector<double> w_colm_temp(numParticles,0.0);
Mat ibox;
for (unsigned int index = 0; index < numParticles; index++) {
// get the bounding box size
int xcbox = particles_new[index][X_POS]-(int)(width_tracker/2);
int ycbox = particles_new[index][Y_POS]-(int)(height_tracker/2);
Rect cbox(xcbox,ycbox,width_tracker,height_tracker);
if(use_hist==1) { // only take one regions
if ((cbox.x < 0) || (cbox.x+cbox.width > image.cols) || (cbox.y < 0) || (cbox.y+cbox.height > image.rows) )
{
w_col = 0.0000001;
}
else
{
Mat HSVCalc;
ibox = image(cbox);
computeHSVHist(ibox, HSVCalc);
w_col = compareHist(HSVinterpolated, HSVCalc, CV_COMP_CORREL);
HSVCalc.release();
}
w_sum += w_col;
w_colm_temp.push_back(w_col);
}
else if (use_hist==2){ // only take three regions
if ((cbox.x < 0) || (cbox.x+cbox.width > image.cols) || (cbox.y < 0) || (cbox.y+cbox.height > image.rows) )
{
w_col = 0.0000001;
}
else{
vector<Mat> HSVCalc;
ibox = image(cbox);
vector<Mat> MultiIMHSV;
Mat tempHSV;
int third = ibox.rows/3;
tempHSV = ibox.rowRange(0, third);
MultiIMHSV.push_back(tempHSV);
tempHSV = ibox.rowRange(third,third+third);
MultiIMHSV.push_back(tempHSV);
tempHSV = ibox.rowRange(third+third, ibox.rows);
MultiIMHSV.push_back(tempHSV);
for (unsigned int hs = 0; hs < MultiHSVTemplate.size(); hs++) {
Mat HSTemp;
computeHSVHist(MultiIMHSV[hs], HSTemp);
HSVCalc.push_back(HSTemp);
}
w_col = 0.0;
for (unsigned int hs = 0; hs < MultiHSVTemplate.size(); hs++) {
int wtemp = compareHist(MultiHSVInterpolated[hs], HSVCalc[hs], CV_COMP_INTERSECT);
w_col += wtemp;
}
}
w_sum += w_col;
w_colm_temp.push_back(w_col);
}
else if(use_hist == 3){ // divide into three regions and overlap
// if out of image bound
if ((cbox.x < 0) || (cbox.x+cbox.width > image.cols) || (cbox.y < 0) || (cbox.y+cbox.height > image.rows) )
{
w_col = 0.0000001;
}
else{
vector<Mat> HSVCalc(3);
ibox = image(cbox);
vector<Mat> MultiIMHSV(3);
int third = ibox.rows/3;
int half = ibox.rows/2;
// Partition step
MultiIMHSV.at(0) = ibox.rowRange(0, half);
MultiIMHSV.at(1) = ibox.rowRange(third,half+third);
MultiIMHSV.at(2) = ibox.rowRange(half, ibox.rows);
// Calculate histogran for every region
Mat HSTemp;
for (unsigned int hs = 0; hs < MultiHSVTemplate.size(); hs++) {
computeHSVHist(MultiIMHSV[hs], HSTemp);
normalize(HSTemp, HSTemp, 0, HSTemp.rows, NORM_MINMAX, -1, Mat() );
HSVCalc.at(hs) = HSTemp;
}
// calculate the weight by comparing them
w_col = 0.0;
for (unsigned int hs = 0; hs < MultiHSVTemplate.size(); hs++) {
double wtemp = compareHist(MultiHSVInterpolated[hs], HSVCalc[hs], CV_COMP_BHATTACHARYYA);
wtemp = 1.0 - wtemp;
w_col += wtemp;
w_col /= (int)(MultiHSVTemplate.size());
}
}
w_sum += w_col;
w_colm_temp.at(index) = w_col;
}
else if(use_hist==4){
}
else{
w_colm_temp.push_back(0.0);
w_sum = 1.0;
}
}
if (!rely_detection) { //dont rely on detection if there is an occlusion
c_det = m_c_det;
c_col = m_c_col;
}
// normalize weight
for (unsigned int index = 0; index < numParticles; index++) {
w_colm.at(index) = w_colm_temp.at(index)/w_sum; //diubah
}
//accumulate
for (unsigned int index = 0; index < numParticles; index++) {
w_det = w_detm.at(index) * c_det;
w_detm.at(index) = w_det;
}
for (unsigned int index = 0; index < numParticles; index++) {
w_col = w_colm.at(index) * c_col;
w_colm.at(index) = w_col;
}
// total weight
for (unsigned int index = 0; index < numParticles; index++) {
weight.at(index) = w_detm.at(index) + w_colm.at(index);
}
}
bool csBox3::ProjectBox (const csTransform& trans, float fov,
float sx, float sy, csBox2& sbox, float& min_z, float& max_z) const
{
const csVector3& origin = trans.GetOrigin ();
int idx = CalculatePointSegment (origin);
const Outline &ol = outlines[idx];
int num_array = MIN (ol.num, 6);
min_z = 100000000.0;
max_z = 0;
csBox3 cbox (trans * GetCorner (ol.vertices[0]));
int i;
// We go to 8 so that we can calculate the correct min_z/max_z.
// If we only go to num_array we will only calculate min_z/max_z
// for the outine vertices.
for (i = 1; i < 8; i++)
{
csVector3 v = trans * GetCorner (ol.vertices[i]);
if (i < num_array)
{
cbox.AddBoundingVertexSmart (v);
min_z = cbox.MinZ ();
max_z = cbox.MaxZ ();
}
else
{
if (v.z < min_z) min_z = v.z;
if (v.z > max_z) max_z = v.z;
}
}
if (max_z < 0.01) return false;
// @@@ In theory we can optimize here again by calling CalculatePointSegment
// again for the new box and the 0,0,0 point. By doing that we could
// avoid doing four perspective projections.
// If z < .1 we do conservative clipping. Not correct but it will generate
// a box that is bigger then the real one which is ok for testing culling.
csVector2 oneCorner;
if (cbox.Max ().z < .1)
PerspectiveWrong (cbox.Max (), oneCorner, fov, sx, sy);
else
Perspective (cbox.Max (), oneCorner, fov, sx, sy);
sbox.StartBoundingBox (oneCorner);
csVector3 v (cbox.MinX (), cbox.MinY (), cbox.MaxZ ());
if (v.z < .1)
PerspectiveWrong (v, oneCorner, fov, sx, sy);
else
Perspective (v, oneCorner, fov, sx, sy);
sbox.AddBoundingVertexSmart (oneCorner);
if (cbox.Min ().z < .1)
PerspectiveWrong (cbox.Min (), oneCorner, fov, sx, sy);
else
Perspective (cbox.Min (), oneCorner, fov, sx, sy);
sbox.AddBoundingVertexSmart (oneCorner);
v.Set (cbox.MaxX (), cbox.MaxY (), cbox.MinZ ());
if (v.z < .1)
PerspectiveWrong (v, oneCorner, fov, sx, sy);
else
Perspective (v, oneCorner, fov, sx, sy);
sbox.AddBoundingVertexSmart (oneCorner);
return true;
}
void browse_map(_wintype* window, CLocation Loc)
{
int ch;
stack_window();
cbox(window, BORDER_COLOR);
show_window(window);
_wintype* mapWindow = _derwin(window, _getheight(window) - 2,
_getwidth(window) - 2, 1, 1);
do
{
draw_map(mapWindow, Loc);
switch (ch = _getch())
{
case _KEY_UP:
Loc.y--;
break;
case _KEY_DOWN:
Loc.y++;
break;
case _KEY_LEFT:
Loc.x--;
break;
case _KEY_RIGHT:
Loc.x++;
break;
case KEY_PGUP:
//do
if (Loc.floor > 0)
Loc.floor--;
else
{
if (Loc.donjon == 0)
Loc.donjon = Dungeon.size() - 1;
else
Loc.donjon--;
Loc.floor = Loc.dungeon().floors - 1;
Loc.y = MAP_SIZE / 2;
Loc.x = MAP_SIZE / 2;
}
//while ((Dungeon[Loc.dungeon].Map[Loc.floor].tiles == 1) &&
// (Dungeon[Loc.dungeon].domain != DOM_OVERWORLD));
break;
case KEY_PGDN:
//do
if (Loc.floor < Loc.dungeon().floors - 1)
Loc.floor++;
else
{
if (Loc.donjon == Dungeon.size() - 1)
Loc.donjon = 0;
else
Loc.donjon++;
Loc.floor = 0;
Loc.y = MAP_SIZE / 2;
Loc.x = MAP_SIZE / 2;
}
//while ((Dungeon[Loc.dungeon].Map[Loc.floor].tiles == 1) &&
// (Dungeon[Loc.dungeon].domain != DOM_OVERWORLD));
break;
}
} while (ch != _KEY_CANCEL);
unstack_window();
}
