regiontracker.cc

不错的shijuegezong的程序

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//  RegionTracker.cc                                                         //
//                                                                           //
//  This tracking class tracks regions from frame to frame                   //
//                                                                           //
//  Author    : Nils T Siebel (nts)                                          //
//  Created   : Mon Oct 22 11:21:21 BST 2001                                 //
//  Revision  : 0.0 of Mon Oct 22 11:21:21 BST 2001                          //
//  Copyright : The University of Reading                                    //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#include <cassert>

#include "RegionTracker.h"

#ifndef NO_DISPLAY
#ifdef DEBUG
#include "os_specific_things.h"  // for sleep()
#endif
#endif

#include "Tracking.h"
#include "MotionDetector.h"
#include "Inputs.h"
#include "text_output.h"
#include "Results.h"
#include "MultiBackgroundSource.h"

#include "tracker_defines_types_and_helpers.h"

namespace ReadingPeopleTracker
{

static const char *RegionTracker_Revision = "@(#) RegionTracker.cc, rev 0.0 of Mon Oct 22 11:21:21 BST 2001, Author Nils T Siebel, Copyright (c) 2001 The University of Reading";


void RegionTracker::predict_old_objects(Results *results)
{
    cdebug << "RT: RegionTracker::predict_old_objects() " << endl;
    
    ListNode<TrackedObject> *curr_obj;
    frame_id_t frame_id = results->get_motion_image()->get_frame_id();
    
    // get regions from tracked object set
    for (curr_obj = results->get_tracked_objects()->first; curr_obj != NULL;
	 curr_obj = curr_obj->next)	
    {
	// predict all regions for this object
	ListNode<Region> *curr;
	
	for (curr = curr_obj->dat->regions->first; curr != NULL; curr = curr->next)
	{
	    // if object integrated into the background, do not change predictions
	    if (curr->dat->incorporated_into_background)
	    {
		curr->dat->source = PREDICTION;
		curr->dat->is_visible = true;
		// curr->dat->frame_last_detected = frame_id - 1;
		
		assert(curr_obj->dat->regions->no_items == 1);  // FIXME: other cases to be implemented
		curr_obj->dat->frames_static++;
		
		continue;
	    }
	    
	    // do not predict new objects (from MotionDetector)
	    if (curr->dat->frame_first_detected == frame_id)
	    {
		cdebug << "RT: RegionTracker::predict_old_objects(): new region? "
		       << "object id " << curr_obj->dat->id << " :- "
		       << endl << *curr->dat << endl;
		
		assert(curr_obj->dat->id == 0); // new measurements have not yet been assigned an id
		
		curr->dat->old_origin = curr->dat->origin;
		curr->dat->direction = Point2(0,0);
		assert (curr->dat->source == MEASUREMENT); // this should have been set earlier
		assert (curr->dat->is_visible == true); // this should have been set earlier
		
		cdebug << "RT: RegionTracker::predict_old_objects(): changed to :- "
		       << endl << *curr->dat << endl;
		continue;
	    }
	    
	    cdebug << "RT: RegionTracker::predict_old_objects(): old region "
		   << "object id " << curr_obj->dat->id << " :- "
		   << endl << *curr->dat << endl;
	    
	    // get new `direction' guess if there are enough meausurements
	    if (curr->dat->frame_last_detected + 1 == frame_id)
	    {
		// detected previous frame, make proper prediction
		cdebug << "RT: RegionTracker::predict_old_objects(): "
		       << "region detected in previous frame, adjusting direction "
		       << endl;
		
		assert(curr->dat->source == MEASUREMENT); // otherwise data is inconsistent
		
		// check whether the update of direction would give a good result
		if ((curr->dat->old_origin == curr->dat->origin) &&
		    (curr->dat->direction != Point2(0,0)))
		{
		    // probably no real measurement (what else?), just slow down prediction
//		    curr->dat->direction /= 2;
		}
		else
		{
		    // we have a valid (measured) new origin so update direction guess
		    curr->dat->direction = curr->dat->origin - curr->dat->old_origin;
		}
	    }
	    else
	    {
		cdebug << "RT: RegionTracker::predict_old_objects(): not seen in previous frame "
		       << frame_id - 1 << " but in " << curr->dat->frame_last_detected << endl;
		assert(curr->dat->source == PREDICTION); // otherwise data is inconsistent
	    }
	    
	    // do actual prediction: very simple constant velocity assumption
	    curr->dat->old_origin = curr->dat->origin;
	    curr->dat->origin += curr->dat->direction;		
	    
	    // mark region as predicted
	    curr->dat->source = PREDICTION;
	    curr->dat->is_visible = false;  // we do not know yet
	    
	    cdebug << "RT: RegionTracker::predict_old_objects(): "
		   << "predicted region in object " << curr_obj->dat->id
		   << ": direction = " << curr->dat->direction
		   << endl
		   << " now object looks like this: " << endl
		   << *curr->dat << endl;
	}
    }
}

void RegionTracker::track_old_objects(Inputs *inputs, Results *results)
{
    cdebug << "RT: RegionTracker::track_old_objects() " << endl;    
    
    // first, initialise all matched tags to false.  As we match predictions
    // to measurements in split_and_merge_regions() and match_regions_in_objects(),
    // these will be queried and set to false later.
    assert (false == 0); // otherwise we cannot use memset in the following way
    memset ((void *) &matched_tag, 0, sizeof matched_tag);
    
    // split and merge measurements and match these hypotheses to predictions
    // using a constructive approach
    split_and_merge_regions(results);
    
    // calculate a region difference measure to compare preditions with measurements
    calculate_region_differences(results);
    
    // match the measurements not split/merged to the predictions not matched
    match_regions_in_objects(inputs, results);
    
}


void RegionTracker::detect_new_objects(Inputs *inputs, Results *results)
{
    cdebug << "RT: RegionTracker::detect_new_objects() " << endl;
    
    // all new objects are already there and tracked (ie compared
    //  to predictions) --- at the same time as the old objects.
}

// tracking loop for each new image
void RegionTracker::process_frame(Inputs *inputs,
				  Results *results,
				  unsigned int max_objects)
{
    predict_old_objects(results);    
    track_old_objects(inputs, results);
    detect_new_objects(inputs, results);
}


//  realno track_distance(Region* reg1, Region* reg2)
//  {
//      Point2 diff = reg1->origin - reg2->origin;
//      realno w = (reg1->width + reg2->width) / 2;
//      realno h = (reg1->height + reg2->height) / 2;
//    
//      realno dw = fabs(reg1->width - reg2->width) / w;
//      realno dh = fabs(reg1->height - reg2->height) / h;
//      realno dl = diff.length() / fmax(w,h);
//    
//      if (dw > 1.0 || dh > 1.0 || dl > 1.0)
//  	return 1000000;
//    
//      return dw + dh + dl;
//  }

void RegionTracker::register_configuration_parameters()
{
    // FIXME: this is a replication of code in BaseTracker::BaseTracker(). Why is this necessary???
    max_objects = 
	configuration.register_int("MAX_OBJECTS", 32,
				   (int *) &max_objects,  false,
				   "RegionTracker", 
				   "The maximum number of regions to track");
    
    debug_level =
	configuration.register_int("DEBUG_LEVEL", 0,
				   (int *) &debug_level, false,
				   "RegionTracker",
				   "General debug level");
    
    // our things
    max_match_difference = 
	configuration.register_int("MAX_MATCH_DIFFERENCE", 70,
				   (int *) &max_match_difference, true,
				   "RegionTracker",
				   "Maximum pixel difference between regions to match");
    
    max_merge_difference = 
	configuration.register_real("MAX_MERGE_DIFFERENCE", 30.0,
				    &max_merge_difference, true,
				    "RegionTracker",
				    "Maximum pixel difference between regions to split a measured moving region into 2 in order to match predictions ");
    
    static_count_incorporate =
	configuration.register_int("STATIC_COUNT_INCORPORATE", 5,
				   (int *) &static_count_incorporate, true,
				   "RegionTracker",
				   "Incorporate static objects into the background after this many frames.");
}

RegionTracker::RegionTracker (Calibration *the_calibration,
			      MotionDetector *the_motion_detector,
			      char *config_filename)
{
    calibration = the_calibration;
    motion_detector = the_motion_detector;
    
    // register configuration parameters with configuration manager
    register_configuration_parameters();
    
    // read values from configuration file
    if (config_filename != NULL)
	configuration.parse_parameter_file(config_filename);
    else
	cerror << "RT: RegionTracker: Warning: No parameter file specified. "
	       << "using default parameter values. \n";
}

// split and merge measurements in order to create hypotheses matching the
//    predictions, using a constructive approach.
// newly created hypotheses are marked as source = SPLITTING and added to
//    our observations --- no existing regions are matched/changed yet.
void RegionTracker::split_and_merge_regions(Results *results)
{
    cdebug << "RT: RegionTracker::split_and_merge_regions() " << endl;
    cdebug << "     FIXME: only splitting into 2 for now " << endl;
    cdebug << "     FIXME: only splitting for now " << endl;
    
    // Try to match pairs of predicted regions predicted to be close to each
    //   other to measurements which could be containing both blobs.
    //
    // If matched, the measurement will be removed and `matched_tag's set.
    
    // assumes that matched_tag [a] has been set to false for all non-matched regions a
    
    TrackedObjectSet *objects = results->get_tracked_objects();
    
    int index1, index2, index3;  // indices into the TrackedObjectSet *objects
    
    Region *prediction1;   // matching...
    Region *prediction2;   //     ...
    Region *measurement;   // ...objects
    
    const int distance_epsilon = 20;  // maximum distance to consider regions close
    
    //
    // get all pairs of predictions that are close enough to be considered
    //
    for (index1 = 0; index1 + 1 < objects->no_items; index1++)
    {
	TrackedObject *obj1 = (*objects)[index1];
	
	// make sure obj1 has regions
	if (obj1->regions->no_items == 0)
	    continue;
	
	// don't try to match regions already matched
	if (matched_tag[index1])
	    continue;
	
	// get region prediction1
	prediction1 = obj1->regions->first->dat;  // FIXME: don't use first only
	
	// prediction1 should be a prediction
	if (prediction1->source != PREDICTION)
	    continue;	
	
	if (prediction1->incorporated_into_background)  // do not split to match these // FIXME: ?
	    continue;
	
	// in order to try all pairs of predictions: get a 2nd one
	for (index2 = index1 + 1; index2 < objects->no_items; index2++)
	{
	    TrackedObject *obj2 = (*objects)[index2];
	    
	    // make sure obj2 has regions
	    if (obj2->regions->no_items == 0)
		continue;
	    
	    // don't try to match regions already matched
	    if (matched_tag[index2])
		continue;
	    
	    // get region prediction2
	    prediction2 = obj2->regions->first->dat;  // FIXME: don't use first only
	    
	    // prediction2 should be a prediction
	    if (prediction2->source != PREDICTION)
		continue;
	    
	    if (prediction2->incorporated_into_background)  // do not split to match these // FIXME: ?
		continue;
	    
	    // don't try to combine regions for matching unless they are close...
	    int hdistance;
	    int vdistance;
	    
	    // calculate horizontal and vertical distance
	    region_distance(prediction1, prediction2, &hdistance, &vdistance);
	    
	    if (vdistance > distance_epsilon)
		continue;
	    
	    //
	    //  now we have two close predictions.  combine into one:
	    //
	    realno combined_xlo = min(prediction1->xlo, prediction2->xlo);
	    realno combined_xhi = max(prediction1->xhi, prediction2->xhi);
	    realno combined_ylo = min(prediction1->ylo, prediction2->ylo);
	    realno combined_yhi = max(prediction1->yhi, prediction2->yhi);
	    Region combined_region((int) combined_xlo, (int) combined_xhi,
				   (int) combined_ylo, (int) combined_yhi);
	    
	    // FIXME: what happens if static objects are matched?
	    // FIXME: check visibility?  could be ok without!
	    
	    //
	    //  compare combined regions to all measurements:
	    //
	    
	    for (index3 = 0; index3 < objects->no_items; index3++)
	    {
		TrackedObject *obj3 = (*objects)[index3];
		
		// make sure obj3 has regions
		if (obj3->regions->no_items == 0)
		    continue;
		
		// don't try to match regions already matched
		if (matched_tag[index3])
		    continue;
		
		// get region measurement
		measurement = obj3->regions->first->dat;  // FIXME: don't use first only
		
		// measurement should be a measurement
		if (measurement->source != MEASUREMENT)
		    continue;
		
		assert(obj3->regions->no_items == 1);   // assume "pure" new measurement
		assert(obj3->profiles->no_items == 0);  // anything else would make no sense
		
		// check the region difference between combined region and measurement
		const realno region_diff = region_difference(&combined_region, measurement);
		
		if (region_diff > max_merge_difference)
		    continue;  // to far apart => mismatch, try with next measurement
		
		//
		//  combined regions do match new measurement.
		//  now create "synthetic" measurements by splitting the large region into 2
		//
		cdebug << "RT: RegionTracker::split_and_merge_regions(): "
		       << "combined regions match new measurement! " << endl;
		
		cdebug << "--- prediction 1: object id " << obj1->id << " " << endl
		       << *obj1->regions->first->dat << endl;
		cdebug << "--- prediction 2: object id " << obj2->id << " " << endl
		       << *obj2->regions->first->dat << endl;
		cdebug << "--- measurement: object id " << obj3->id << "  " << endl
		       << *obj3->regions->first->dat << endl;
		// create new TrackedObject, copying the data from prediction1
		TrackedObject *new_obj1 = objects->add_a_copy(obj1);
//		new_obj1->id = 0;                                     // but not the id
		Region *new_reg1 = new_obj1->regions->first->dat;     // get the region
		
		// create new TrackedObject, copying the data from prediction1
		TrackedObject *new_obj2 = objects->add_a_copy(obj2);
//		new_obj2->id = 0;                                     // but not the id
		Region *new_reg2 = new_obj2->regions->first->dat;     // get the region
		
		// adjust positions of new_reg1 and new_reg2, assuming size has not changed
		// FIXME: maybe it has become smaller?  is that possible?
		
		// let edges "snap" to edges of measurement
		const int max_adjustment = 50;   // max pos. shift in pixels, outward only
		int delta_x_l, delta_x_r, delta_y_l, delta_y_r;  // delta x/y left/right
		
		// new_reg1: let edges "snap" to edges of measurement
		//  1 - shift horizontally
		delta_x_l = measurement->xlo - new_reg1->xlo;