/* 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);
}
}
