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; }