void CovariancePatchDescriptor::CalculateCov() {
vector<Mat> F(tracker_param->numFeature);
if (tracker_param->isRGB) {
makeFrgb(this->m_imgPatch, F,tracker_param);
}
else {
makeFgray(this->m_imgPatch, F,tracker_param);
}
//makeF(this->m_imgPatch, F,tracker_param);
//printMat(F,MAT_TYPE_FLOAT);
vector<Mat> P(tracker_param->numFeature);
vector< vector<Mat> > Q(tracker_param->numFeature, vector<Mat>(tracker_param->numFeature));
makePQ(F, P,Q,tracker_param);
//printMat(P,MAT_TYPE_DOUBLE);
Mat cov = Mat(tracker_param->numFeature,tracker_param->numFeature,CV_64F);
CovarianceRegion(P, Q, cov,tracker_param);
//printMat(Crxy,MAT_TYPE_DOUBLE);
//
//WE CALCULATE DIFFERENT LOG-MAPS, FOR DIFFERENT IMAGE PATCH'S AREAS
//
Mat covAux;
int halfw,halfh,h,w;
if (this->tracker_param->normType == FACE) {
halfw = halfh = SIZE_NORM_FACE/2;
h = w = SIZE_NORM_FACE;
}
else {
halfh = SIZE_NORM_PEDESTRIAN_H/2;
halfw = SIZE_NORM_PEDESTRIAN_W/2;
h = SIZE_NORM_PEDESTRIAN_H;
w = SIZE_NORM_PEDESTRIAN_W;
}
if (!this->m_descp.empty()) this->m_descp.clear();
this->m_descp.resize(5);
//FULL PATCH
this->m_descp[0] = mapLogMat(cov,tracker_param);
//LEFT HALF PATCH
covAux = Mat(tracker_param->numFeature,tracker_param->numFeature,CV_64F);
//CovarianceRegion(P, Q, covAux,Point2f(0,0), Point2f(halfw-1,h-1));
CovarianceRegion(P, Q, covAux,Point2f(0,0), Point2f(halfw-1,halfh-1),tracker_param);
this->m_descp[1] = mapLogMat(covAux, tracker_param);
//RIGHT HALF PATCH
covAux = Mat(tracker_param->numFeature,tracker_param->numFeature,CV_64F);
//CovarianceRegion(P, Q, covAux,Point2f(halfw,0), Point2f(w-1,h-1));
CovarianceRegion(P, Q, covAux,Point2f(halfw,0), Point2f(w-1,halfh-1),tracker_param);
this->m_descp[2] = mapLogMat(covAux,tracker_param);
//BOTTOM HALF PATCH
covAux = Mat(tracker_param->numFeature,tracker_param->numFeature,CV_64F);
//CovarianceRegion(P, Q, covAux,Point2f(0,halfh), Point2f(w-1,h-1));
CovarianceRegion(P, Q, covAux,Point2f(0,halfh), Point2f(halfw-1,h-1),tracker_param);
this->m_descp[3] = mapLogMat(covAux,tracker_param);
//TOP HALF PATCH
covAux = Mat(tracker_param->numFeature,tracker_param->numFeature,CV_64F);
//CovarianceRegion(P, Q, covAux,Point2f(0,0), Point2f(w-1,halfh-1));
CovarianceRegion(P, Q, covAux,Point2f(halfw,halfh), Point2f(w-1,h-1),tracker_param);
this->m_descp[4] = mapLogMat(covAux,tracker_param);
//borrando
cov.release();
covAux.release();
P.clear();
Q.clear();
this->m_imgPatch.release();
}
/*
* Function: aStart
*
* @Parameters: Grid_Container grid ~ grid_container to find neighboors
* Point start ~ starting point
* Point goal ~ goal point
*
* Uses an pseudocode I found of wikipedia as guidance on my implementation.
*
* @returns an array of points which contains the path to the goal from the start point
*
*
*/
ArrayList* aStar( Grid_Container *grid, Point start, Point goal)
{
int i = 0;
ArrayList *closed_set = NULL;
PQ *open_set = NULL;
ArrayList *map = NULL;
start.g_score = 0;
start.f_score = start.g_score + heuristic_estimate( start, goal);
open_set = makePQ();
closed_set = makeArrayList();
map = makeArrayList();
insert( open_set, start);
while ( !isEmpty( open_set))
{
Point current = pop(open_set);
//printf("Current x: %d y: %d\n", current.x, current.y);
if ( isGoal( current))
{
ArrayList* rtrnList = NULL;
free_PQ( open_set);
free_ArrayList(closed_set);
rtrnList = reconstruct_Path(map, current);
free_ArrayList( map);
return rtrnList;
}
add(closed_set, current);
ArrayList *neighbors = NULL;
neighbors = getNeighborPoints(current, grid);
//printf("Printing neighbors:\n");
//printArrayList(neighbors);
for ( i = 0; i < getSize( neighbors); i++)
{
Point neighbor = get( neighbors, i);
if ( !contains( closed_set, neighbor))
{
int tentative_g_score = calculate_g_score(current, neighbor);
if (!containsPoint( open_set, neighbor) || tentative_g_score < neighbor.g_score)
{
neighbor.parent_x = current.x;
neighbor.parent_y = current.y;
add( map, current);
neighbor.g_score = tentative_g_score;
if (!isGoal(neighbor))
{
neighbor.f_score = neighbor.g_score + heuristic_estimate( current, neighbor);
}
else
{
neighbor.f_score = 0;
}
if (!containsPoint( open_set, neighbor))
{
insert( open_set, neighbor);
}
}
}
}
free_ArrayList(neighbors);
}
return NULL;
}
