regiontracker.cc

不错的shijuegezong的程序

		delta_x_r = measurement->xhi - new_reg1->xhi;
		
		if ((delta_x_l > -max_adjustment) &&    // limit outward movement only
		    (abs(delta_x_l) < abs(delta_x_r)))  // we are closer to the left
		{
		    // shift horizontally to match measurement->xlo
		    new_reg1->xlo += delta_x_l;
		    new_reg1->xhi += delta_x_l;
		    new_reg1->origin.x += delta_x_l;
		    
		    // update movement prediction
		    new_reg1->old_origin.x = new_reg1->origin.x - delta_x_l;
		    new_reg1->direction.x = delta_x_l;
		}
		else
		    if ((delta_x_r < max_adjustment) &&      // limit outward movement only 
			(abs(delta_x_l) >= abs(delta_x_r)))  // we are closer to the right
		    {
			// shift horizontally to match measurement->xhi
			new_reg1->xlo += delta_x_r;
			new_reg1->xhi += delta_x_r;
			new_reg1->origin.x += delta_x_r;
			
			// update movement prediction
			new_reg1->old_origin.x = new_reg1->origin.x - delta_x_r;
			new_reg1->direction.x = delta_x_r;
		    }
		
		//  2 - shift vertically
		delta_y_l = measurement->ylo - new_reg1->ylo;
		delta_y_r = measurement->yhi - new_reg1->yhi;
		
		if ((abs(delta_y_l) < max_adjustment) &&
		    (abs(delta_y_l) < abs(delta_y_r)))
		{
		    // shift vertically to match measurement->ylo
		    new_reg1->ylo += delta_y_l;
		    new_reg1->yhi += delta_y_l;
		    new_reg1->origin.y += delta_y_l;
		}
		else
		    if (abs(delta_y_r) < max_adjustment)
		    {
			// shift vertically to match measurement->yhi
			new_reg1->ylo += delta_y_r;
			new_reg1->yhi += delta_y_r;
			new_reg1->origin.y += delta_y_r;
			
			// update movement prediction
			new_reg1->old_origin.y = new_reg1->origin.y - delta_y_r;
			new_reg1->direction.y = delta_y_r;
		    }
		
		// new_reg2: let edges "snap" to edges of measurement
		//  1 - shift horizontally
		delta_x_l = measurement->xlo - new_reg2->xlo;
		delta_x_r = measurement->xhi - new_reg2->xhi;
		
		if ((delta_x_l > -max_adjustment) &&    // limit outward movement only
		    (abs(delta_x_l) < abs(delta_x_r)))  // we are closer to the left
		{
		    // shift horizontally to match measurement->xlo
		    new_reg2->xlo += delta_x_l;
		    new_reg2->xhi += delta_x_l;
		    new_reg2->origin.x += delta_x_l;
		    
		    // update movement prediction
		    new_reg2->old_origin.x = new_reg2->origin.x - delta_x_l;
		    new_reg2->direction.x = delta_x_l;
		}
		else
		    if ((delta_x_r < max_adjustment) &&      // limit outward movement only 
			(abs(delta_x_l) >= abs(delta_x_r)))  // we are closer to the right
		    {
			// shift horizontally to match measurement->xhi
			new_reg2->xlo += delta_x_r;
			new_reg2->xhi += delta_x_r;
			new_reg2->origin.x += delta_x_r;
			
			// update movement prediction
			new_reg2->old_origin.x = new_reg2->origin.x - delta_x_r;
			new_reg2->direction.x = delta_x_r;
		    }
		
		//  2 - shift vertically
		delta_y_l = measurement->ylo - new_reg2->ylo;
		delta_y_r = measurement->yhi - new_reg2->yhi;
		
		if ((abs(delta_y_l) < max_adjustment) &&
		    (abs(delta_y_l) < abs(delta_y_r)))
		{
		    // shift vertically to match measurement->ylo
		    new_reg2->ylo += delta_y_l;
		    new_reg2->yhi += delta_y_l;
		    new_reg2->origin.y += delta_y_l;
		}
		else
		    if (abs(delta_y_r) < max_adjustment)
		    {
			// shift vertically to match measurement->yhi
			new_reg2->ylo += delta_y_r;
			new_reg2->yhi += delta_y_r;
			new_reg2->origin.y += delta_y_r;
			
			// update movement prediction
			new_reg2->old_origin.y = new_reg2->origin.y - delta_y_r;
			new_reg2->direction.y = delta_y_r;
		    }
		
		// adjust region image if necessary: new_reg1
		if (new_reg1->region_img != NULL)
		{
		    delete new_reg1->region_img;
		    Image *motion_image = results->get_motion_image();
		    
  		    unsigned int xmin = max(0, new_reg1->xlo);
  		    unsigned int xmax = min(motion_image->get_width() - 1,
					    (unsigned int) new_reg1->xhi);
  		    unsigned int ymin = max(0, new_reg1->ylo);
  		    unsigned int ymax = min(motion_image->get_height() - 1,
					    (unsigned int) new_reg1->yhi);
		    
  		    new_reg1->region_img =
  			motion_image->extract_subimage(xmin,xmax,ymin,ymax);
		}
		
		// adjust region image if necessary: new_reg2
		if (new_reg2->region_img != NULL)
		{
		    delete new_reg2->region_img;
		    Image *motion_image =
			results->get_motion_image();
		    
  		    unsigned int xmin = max(0, new_reg2->xlo);
  		    unsigned int xmax = min(motion_image->get_width() - 1,
					    (unsigned int) new_reg2->xhi);
		    unsigned int ymin = max(0, new_reg2->ylo);
  		    unsigned int ymax = min(motion_image->get_height() - 1,
					    (unsigned int) new_reg2->yhi);
		    
  		    new_reg2->region_img =
  			motion_image->extract_subimage(xmin,xmax,ymin,ymax);
		}
		
		// adjust status
		new_reg1->frame_last_detected =
		    new_reg2->frame_last_detected =
		    measurement->frame_last_detected;             // probably now
		
		new_reg1->time_last_detected =
		    new_reg2->time_last_detected =
		    measurement->time_last_detected;             // probably now
		
		new_reg1->is_visible =
		    new_reg2->is_visible =
		    measurement->is_visible;                      // probably true
		
		new_reg1->source =
		    new_reg2->source = SPLITTING;
		
		// NB: also adjust the measurement's source so it might be removed later
		measurement->source = SPLITTING;
		
#ifdef DEBUG
		cdebug << "--- split 1:     " << endl << *new_obj1->regions->first->dat << endl;
		cdebug << "--- split 2:     " << endl << *new_obj2->regions->first->dat << endl;
		cdebug << "--- measurement: " << endl << *obj3->regions->first->dat << endl;
		
#ifndef NO_DISPLAY
		if (debug_level == 1)
		{
		    // draw predictions
		    cdebug << " drawing: " << flush;
// FIXME		    inputs->get_video_image()->display();   // ie clear...
		    linewidth(1);
		    Image::set_colour(obj1->id);
		    cdebug << "prediction1, " << flush;
		    prediction1->draw_box();
		    gflush();
		    sleep(2);
		    Image::set_colour(obj2->id);
		    cdebug << "prediction2, " << flush;
		    prediction2->draw_box();
		    gflush();
		    sleep(2);
		    
		    // draw measurement
		    Image::set_colour(0);
		    cdebug << "measurement, " << flush;
		    measurement->draw_box();
		    gflush();
		    sleep(2);
		    
		    // draw new (split) regions
		    linewidth(2);
		    Image::set_colour(0);
		    cdebug << "split1, " << flush;
		    new_reg1->draw_box();
		    gflush();
		    sleep(2);
		    cdebug << "split2. " << endl << flush;
		    new_reg2->draw_box();
		    gflush();
		    sleep(5);
		}
#endif   // ifndef NO_DISPLAY
#endif   // ifdef DEBUG
	    }
	}
    }
}

void RegionTracker::calculate_region_differences(Results *results)
{
    cdebug << "RT: RegionTracker::calculate_region_differences() " << endl;
    
    // calculate a difference measure for all pairs (r1,r2) of regions in the
    //   TrackedObjectSet (objects), r1 in obj1 and r2 in obj2 with
    //   obj1 being a PREDICTION and obj2 being a MEASUREMENT such that
    //   region_diff[index1][index2] will give a reasonable difference measure for
    //   r1 and r2, with obj1 = (*objects) [index1] and obj2 = (*objects) [index2].
    
    TrackedObjectSet *objects = results->get_tracked_objects();
    
    int index1, index2;  // indices into the TrackedObjectSet *objects
    
    //  calculate differences for all suitable pairs of objects/regions
    for (index1 = 0; index1 < objects->no_items; index1++)
    {
	TrackedObject *obj1 = (*objects)[index1];
	
	// make sure obj1 has regions
	if (obj1->regions->no_items == 0)
	    continue;
	
	// get region r1
	Region *r1 = obj1->regions->first->dat;  // FIXME: don't use first only
	
	// r1 should be a prediction
	if (r1->source != PREDICTION)
	    continue;	
	
	// match to all regions in set2
	for (index2 = 0; index2 < objects->no_items; index2++)
	{
	    // don't match object to itself
	    if (index1 == index2)
		continue;
	    
	    TrackedObject *obj2 = (*objects)[index2];
	    
	    // make sure obj2 has regions
	    if (obj2->regions->no_items == 0)
		continue;
	    
	    // get region r2
	    Region *r2 = obj2->regions->first->dat;  // FIXME: don't use first only
	    
	    // r2 should be a measurement or derived from splitting/merging a measurement
	    if ((r2->source != MEASUREMENT) && (r2->source != SPLITTING))
		continue;
	    
	    // now calculate difference measure using position, width and height
	    region_diff [index1] [index2] = region_difference(r1,r2);
	    
	    // add a penalty for static objects so they won't be matched too easily
	    //   (situation: objects moving close to a static one)
	    if (r1->incorporated_into_background)
		region_diff [index1] [index2] += 15;   // FIXME: how about a calculation?
	    
//	    cdebug << " calculated region_diff[" << index1 << "] [" << index2
//		 << "] is " << region_diff [index1] [index2] << endl;
	}
    }
}


// match the measurements not split/merged to the predictions not matched
void RegionTracker::match_regions_in_objects(Inputs *inputs, Results *results)
{
    cdebug << "RT: RegionTracker::match_regions_in_objects() " << endl;
    
    //
    // use the previously calculated region difference measures (in region_diff[])
    //   to find pairs (pred_region,meas_region) of regions in a pair
    //   (pred_object,meas_object) of objects in the TrackedObjectSet objects,
    //   pred_region in pred_object and meas_region in meas_object with
    //   pred_object being a PREDICTION and meas_object being a MEASUREMENT or
    //   created by SPLITTING, and have a look at region_diff[index1][index2]
    //   to see whether the regions match.
    //
    // tracked objects where the regions are matched are assumed to be the same,
    //   the predicted region will be replaced by the new region measurement
    
    // assumes that matched_tag [r] has been set to false for all non-matched regions r
    
    TrackedObjectSet *objects = results->get_tracked_objects();
    
    // pair tested for match: indices, objects and regions
    unsigned int pred_index;  // indices into the TrackedObjectSet *objects
    unsigned int meas_index;
    TrackedObject *pred_object;
    TrackedObject *meas_object;
    Region *pred_region;
    Region *meas_region;
    
    // matching pair: indices, objects and regions
    unsigned int matched_pred_index = 0;
    unsigned int matched_meas_index = 0;
    TrackedObject *matched_pred_object = NULL;
    TrackedObject *matched_meas_object = NULL;
    Region *matched_pred_region = NULL;
    Region *matched_meas_region = NULL;
    
    unsigned int num_matched;
    realno min_difference_so_far;
    bool matched_static;           // whether we matched (and should revive) a static object
    
    // find all matches, best matches first
    do
    {
	num_matched = 0;
	min_difference_so_far = 10000;
	matched_static = false;
	
	for (pred_index = 0; pred_index < objects->no_items; pred_index++)
	{
	    pred_object = (*objects)[pred_index];
	    
	    // make sure pred_object has regions
	    if (pred_object->regions->no_items == 0)
		continue;
	    
	    // don't match regions that have been matched already
	    if (matched_tag[pred_index])  // (could be set by split_and_merge_regions())
		continue;
	    
	    // get region pred_region
	    pred_region = pred_object->regions->first->dat;  // FIXME: don't use first only
	    
	    // pred_region should be a prediction
	    if (pred_region->source != PREDICTION)
		continue;
	    
	    // search all measurements meas_region for combinations (pred_region,meas_region) with minimum/best fit
	    for (meas_index = 0; meas_index < objects->no_items; meas_index++)
	    {
		// don't match object to itself
		if (pred_index == meas_index)
		    continue;
		
		meas_object = (*objects)[meas_index];
		
		// make sure meas_object has regions
		if (meas_object->regions->no_items == 0)
		    continue;
		
		// don't match to regions that have been matched already
		if (matched_tag[meas_index])
		    continue;
		
		// get region meas_region
		meas_region = meas_object->regions->first->dat; // FIXME: don't use first only
		
		// meas_region should be a measurement or derived from splitting/merging measurements
		if ((meas_region->source != MEASUREMENT) && (meas_region->source != SPLITTING))
		    continue;
		
		assert((meas_object->id == 0) || (meas_region->source == SPLITTING)); // check data consistency
		
		// get difference measure from table
		realno difference = region_diff [pred_index] [meas_index];
		
		// check whether difference is the best (minimum) difference so far
		if ((difference < max_match_difference) &&
		    (difference < min_difference_so_far))
		{
		    // match found (best match so far, more to compare...)
		    min_difference_so_far = difference;
		    
		    // remember matching pair...
		    matched_pred_index = pred_index;
		    matched_meas_index = meas_index;
		    matched_pred_object = pred_object;
		    matched_meas_object = meas_object;
		    matched_pred_region = pred_region;
		    matched_meas_region = meas_region;
		    
		    num_matched = 1;
		    matched_static = pred_object->is_static;
		}
  		else
  		{
#ifdef DEBUG
		    cdebug << "RT: RegionTracker::match_regions_in_objects(): "
			   << "ignored bad match dist " << difference
			   << " betw pred " << pred_object->id << " idx " << pred_index;
		    if (meas_region->source == MEASUREMENT)
			cdebug << " to meas ";
		    else
			cdebug << " to split ";
		    cdebug << meas_object->id << " idx " << meas_index << endl;
#endif
		}
	    }
	}
	
	//
	//  if we have found a match we replace the prediction by the measurement
	//
	if (num_matched > 0)  // found a match!
	{