/* randomly re-place one of two points that are too close to each other, * move others that are too far from the centroid towards the centroid */ void OpticalFlow::AddNewFeatures(IplImage* rgbImage, CPointVector& features, CStatusVector& status, int last_width, int last_height, bool use_prob_distr) { ASSERT(m_pProbDistrProvider); CDoubleMatrix distances; DistanceMatrix(features, distances); // discard the highest 15percent of distances when computing centroid int discard_num_distances = (int)(0.15*(double)distances.size()); int centroid_index = FindCentroid(distances, discard_num_distances); CvPoint2D32f centroid = features[centroid_index]; // double halfwidth = last_width/2.0; // double halfheight = last_height/2.0; // double left = centroid.x-halfwidth; // double right = centroid.x+halfwidth; // double top = centroid.y-halfheight; // double bottom = centroid.y+halfheight; for (int fcnt=m_num_features_tracked; fcnt<m_target_num_features; fcnt++) { // try to find a location that was segmented as foreground color int x, y; double prob = 1.0; int prob_cnt = 0; do { x = (int) (centroid.x-last_width/2.0+(rand()%last_width)); y = (int) (centroid.y-last_height/2.0+(rand()%last_height)); x = min(x, rgbImage->width-1); y = min(y, rgbImage->height-1); x = max(x, 0); y = max(y, 0); ColorBGR* color; GetPixel(rgbImage, x, y, &color); if (use_prob_distr) { prob = m_pProbDistrProvider->LookupProb(*color); } prob_cnt++; } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20); features[fcnt].x = (float)x; features[fcnt].y = (float)y; status[fcnt] = 0; } // end for fcnt }
/* randomly re-place one of two points that are too close to each other, * move others that are too far from the centroid towards the centroid */ void OpticalFlow::ConcentrateFeatures(IplImage* rgbImage, CPointVector& features, CStatusVector& status, int last_width, int last_height, bool use_prob_distr) { ASSERT(m_pProbDistrProvider); int round_or_square = 2; // the area in which we will concentrate the features, 1 is round, 2 is rectangular CDoubleMatrix distances; DistanceMatrix(features, distances); // discard the highest 15percent of distances when computing centroid int discard_num_distances = (int)(0.15*(double)distances.size()); int centroid_index = FindCentroid(distances, discard_num_distances); CvPoint2D32f centroid = features[centroid_index]; double halfwidth = last_width/2.0; double halfheight = last_height/2.0; double left = centroid.x-halfwidth; double right = centroid.x+halfwidth; double top = centroid.y-halfheight; double bottom = centroid.y+halfheight; int num_features = (int)features.size(); for (int fcnt=0; fcnt<num_features; fcnt++) { m_feature_status[fcnt] = 1; for (int scnd=fcnt+1; scnd<num_features; scnd++) { double dist; if (fcnt<scnd) { dist = distances[fcnt][scnd]; } else { // scnd<fcnt dist = distances[scnd][fcnt]; } int cnt=0; bool too_close = dist<m_min_distance; while (too_close && cnt<20) { // try to find a location that was segmented as foreground color // and one that's far away enough from all other features int x, y; double prob = 1.0; int prob_cnt = 0; do { x = (int) (centroid.x-last_width/2.0+(rand()%last_width)); y = (int) (centroid.y-last_height/2.0+(rand()%last_height)); x = min(x, rgbImage->width-1); y = min(y, rgbImage->height-1); x = max(x, 0); y = max(y, 0); ColorBGR* color; GetPixel(rgbImage, x, y, &color); if (use_prob_distr) { prob = m_pProbDistrProvider->LookupProb(*color); } prob_cnt++; } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20); features[fcnt].x = (float)x; features[fcnt].y = (float)y; status[fcnt] = 0; too_close = TooCloseToOthers(features, fcnt, (int)features.size()); cnt++; } // end while } // end for scnd if (centroid_index!=fcnt) { if (round_or_square==1) { // round double dist = -1; if (fcnt<centroid_index) { dist = distances[fcnt][centroid_index]; } else if (centroid_index<fcnt) { dist = distances[centroid_index][fcnt]; } else { // will not enter the following loop } while (dist>last_width/2.0) { features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0); features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0); m_feature_status[fcnt] = 0; double dx = features[fcnt].x-centroid.x; double dy = features[fcnt].y-centroid.y; dist = sqrt(dx*dx+dy*dy);; } } else { // rectangular area // move towards horizontal center while (features[fcnt].x<left || features[fcnt].x>right) { features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0); status[fcnt] = 0; } // move towards vertical center while (features[fcnt].y<top || features[fcnt].y>bottom) { features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0); status[fcnt] = 0; } } } // end centroid_index!=fcnt features[fcnt].x = min(max(features[fcnt].x, .0f), rgbImage->width-1.0f); features[fcnt].y = min(max(features[fcnt].y, .0f), rgbImage->height-1.0f); } }
void main(int argc, char *argv[]) { ParseCommandLine (argc, argv); StereoPosData out_data, out_data_dominant; YARPImageOf<YarpPixelMono> out_image; YARPImageOf<YarpPixelMono> out_image_dominant; YARPImageOf<YarpPixelMono> old_image; out_image.Resize (Size, Size); out_image_dominant.Resize (Size, Size); old_image.Resize (Size, Size); memset (old_image.GetRawBuffer(), 0, Size * Size); // register names. for (int i = 0; i < NUMBEROFCHANNELS; i++) { channels[i].Register (portnames[i]); } out_saliency.Register (out_saliency_name); out.Register (out_pos_data_name); out_dominant.Register (out_pos_data_name_dominant); FiveBoxesInARow boxes[NUMBEROFCHANNELS]; for (i = 0; i < NUMBEROFCHANNELS; i++) { memset (boxes+i, 0, sizeof(FiveBoxesInARow)); boxes[i].box1.valid = false; boxes[i].box2.valid = false; boxes[i].box3.valid = false; boxes[i].box4.valid = false; boxes[i].box5.valid = false; } // compute saliency scale factor. int scalefactor = 0; for (i = 0; i < NUMBEROFCHANNELS; i++) scalefactor += relative_weight[i]; scalefactor = 255 / scalefactor; if (scalefactor == 0) scalefactor = 1; YARPTime::DelayInSeconds(1.0); while (1) { // read all the valid boxes and try to fuse them... for (int i = 0; i < NUMBEROFCHANNELS; i++) { if (channels[i].Read(0)) { boxes[i] = channels[i].Content(); } } out_image.Zero (); out_image_dominant.Zero (); // add channel boxes (5) to out_image. for (i = 0; i < NUMBEROFCHANNELS; i++) DrawBoxes (boxes[i], out_image, relative_weight[i], scalefactor); for (i = 0; i < NUMBEROFCHANNELS; i++) { if (source_retina[i] == RETINA_DOMINANT) { DrawBoxes (boxes[i], out_image_dominant, relative_weight[i], scalefactor); } } //TmpFilter (old_image, out_image); int xx = 0, yy = 0; int xx_dom = 0, yy_dom = 0; bool valid = FindCentroid (out_image, &xx, &yy); FindCentroid (out_image_dominant, &xx_dom, &yy_dom); out_data.valid = valid; out_data.xl = xx; out_data.yl = yy; out_data.xr = xx; out_data.yr = yy; out_data_dominant.valid = true; out_data_dominant.xl = xx_dom; out_data_dominant.yl = yy_dom; out_data_dominant.xr = xx_dom; out_data_dominant.yr = yy_dom; YARPGenericImage& outImg = out_saliency.Content(); // Lasts until next Write() outImg.PeerCopy(out_image); out_saliency.Write(); out.Content() = out_data; out.Write(); out_dominant.Content() = out_data_dominant; out_dominant.Write(); delay(30); } }