regiontracker.cc

不错的shijuegezong的程序

#ifdef DEBUG
	    cdebug << "RT: RegionTracker::match_regions_in_objects(): "
		   << "matched pred " << matched_pred_object->id 
		   << " idx " << matched_pred_index;
	    if (matched_meas_region->source == MEASUREMENT)
		cdebug << " to meas ";
	    else
		cdebug << " to split ";
	    cdebug << matched_meas_object->id
		   << " idx " << matched_meas_index
		   << " at difference " << min_difference_so_far << ": " 
		   << endl << "---" << endl;
	    cdebug << *matched_pred_region << "---" << endl;
	    cdebug << *matched_meas_region << "---" << endl;
#endif	    
	    
	    // tag to remember match so we won't match it again
	    matched_tag [matched_pred_index] = true;
	    matched_tag [matched_meas_index] = true;
	    
	    bool object_is_static;
	    
	    // check for static regions and incorporate them
	    //   into the background
	    cdebug << "RT: RegionTracker::match_regions_in_objects(): "
		   << " pred id " << matched_pred_object->id
		   << " diff " << min_difference_so_far
		   << " Euklidean dist "
		   << hypot(matched_pred_region->old_origin.x - matched_meas_region->origin.x,
			    matched_pred_region->old_origin.y - matched_meas_region->origin.y)
		   << " hypoth off by "
		   << hypot(matched_meas_region->origin.x - matched_pred_region->origin.x,
			    matched_meas_region->origin.y - matched_pred_region->origin.y)
		   << ", matched_static == " << matched_static
		   << endl;
	    
	    // check whether we have detected a static object which is not marked as such
//  	    if (hypot(matched_pred_region->old_origin.x - matched_meas_region->origin.x,
//  		      matched_pred_region->old_origin.y - matched_meas_region->origin.y)
	    if (min_difference_so_far
		< fmin(6, fmax(3,matched_meas_region->width / 10)))  // close enough?
	    {
		// the region has not moved much -> is static!
		object_is_static = true;
		matched_pred_object->frames_static++;
		
		cdebug << "RT: RegionTracker::match_regions_in_objects(): "
		       << "found static object id " << matched_pred_object->id
		       << " time static " << matched_pred_object->frames_static
		       << endl;
	    }
	    else
	    {
		object_is_static = false;
		matched_pred_object->frames_static = 0;  // reset
		
		// object is moving.  check whether it has been static an is in background
		if (matched_pred_region->incorporated_into_background)  // started moving again!
		{
		    // remove revived profile from background if MultiBackgroundSource is used
		    if (motion_detector->get_multi_background() != NULL)
		    {
			motion_detector->get_multi_background()
			    -> remove_region(matched_pred_region);
			
			cdebug << "RT: RegionTracker::match_regions_in_objects(): "
			       << "one static object removed from background "
			       << endl;
		    }
		    matched_pred_region->incorporated_into_background = false;
		}
	    }
	    
	    //
	    //  now correct matched prediction
	    //
	    
	    //  copy over new measurements (position, size) to old match
	    //  so we can destroy the new one and use the old	    
	    
	    
	    // use Observation::operator= and do not copy over everything in Region...
	    // NB do not copy over Region::old_origin or Region::incorporated_into_background
	    //    and not frame_first_detected or frame_last_detected
	    
	    // remember some values before copying the rest...
	    frame_id_t old_frame_first_detected =
		matched_pred_region->frame_first_detected;

	    frame_time_t old_time_first_detected =
		matched_pred_region->time_first_detected;

    	    matched_pred_region->Observation::operator=(*matched_meas_region);
	    
	    // set values not set correctly by the operator
	    matched_pred_region->frame_first_detected = old_frame_first_detected;
	    matched_pred_region->time_first_detected = old_time_first_detected;

	    matched_pred_object->is_visible = matched_meas_object->is_visible;   // ie true
	    
	    if (object_is_static)
	    {
		// make no prediction next time to distinguish static and slowly moving objects
		matched_pred_region->old_origin = matched_pred_region->origin;
		matched_pred_region->direction = Point2(0,0);
	    }	    
	    
	    // region image...
	    if (matched_pred_region->region_img != NULL)
		delete matched_pred_region->region_img;
	    
	    matched_pred_region->region_img = matched_meas_region->region_img;
	    matched_meas_region->region_img = NULL;
	    
	    // boundary points...
	    if (matched_pred_region->region_boundary != NULL)
		delete matched_pred_region->region_boundary;
	    
	    matched_pred_region->region_boundary = matched_meas_region->region_boundary;
	    matched_meas_region->region_boundary = NULL;
	    
	    matched_pred_region->source = MEASUREMENT;
	    
	    // keep associations with other type measurements by moving them over...
	    assert(matched_meas_object->regions->no_items == 1); // FIXME: other cases to be implemented
	    matched_pred_object->profiles  -> concat(matched_meas_object->profiles);
	    matched_pred_object->features  -> concat(matched_meas_object->features);  
	    
	    if (object_is_static)
	    {
		//
		// check MultiBackground features if available
		//
		if ((motion_detector->get_multi_background() != NULL) &&
		    (matched_pred_object->frames_static > static_count_incorporate) &&
		    (! matched_pred_region->incorporated_into_background))
		{
		    // temporarily incorporate region into background
		    
		    // in order to add the static object into the background we need
		    //   to create a Region with an Image of the area
		    
		    Image *video_image = inputs->get_video_image();
		    
		    int xmin = max (0, matched_pred_region->xlo - 3);
		    int xmax = min ((int) video_image->get_width() - 1,
				    matched_pred_region->xhi + 3);
		    int ymin = max (0, matched_pred_region->ylo - 3);
		    int ymax = min ((int) video_image->get_height() - 1,
				    matched_pred_region->yhi + 3);
		    
		    Image *static_image =
			video_image->extract_subimage(xmin,xmax,ymin,ymax);
		    
		    Region *static_region = 
			new Region(xmin,xmax,ymin,ymax,static_image);
		    
		    // as static objects in MultiBackgroundSource are identified by
		    //   the origin location, set it to the same value
		    static_region->origin = matched_pred_region->origin;
		    
		    matched_pred_object->frames_static--; // sorry. for better readability elsewhere
		    
		    motion_detector->get_multi_background()->add_region(static_region); // uses List::add()
		
		    cdebug << "RT: RegionTracker::match_regions_in_objects(): "
			   << "static object incorporated into background :- "
			   << endl
			   << *matched_pred_object;
		    
		    matched_pred_region->incorporated_into_background = true;
  		}
	    }
	    
	    //
	    // finally, mark new TrackedObject for deletion simply by deleting
	    //   the RegionSet.  NB don't delete TrackedObject or otherwise meas_index
	    //   becomes incorrect (index on difference table).
	    //
	    matched_meas_object->regions->destroy_all();  // FIXME: What if there is more than 1
	}
    }
    while (num_matched > 0);
    
    cdebug << endl;
}

// calculate a difference measure between two regions, considering positions as well as sizes
realno RegionTracker::region_difference(const Region *r1, const Region *r2)
{
    return (hypot((r1->origin.x - r2->origin.x), (r1->origin.y - r2->origin.y)) +
	    hypot(1.5*(r1->width - r2->width), (r1->height - r2->height))) / 3.216989200105089695361299;

// NB  3.216989200105089695361299 ``normalises'' as it is sqrt (1*1 + 1*1) + srqt (1.5*1.5 + 1*1)
//		 fabs(r1->height - r2->height)) / 2;	    
//  		(hypot(r1->origin.x - r2->origin.x, 5*(r1->origin.y - r2->origin.y)) +
//  		 hypot(r1->width - r2->width, 2*(r1->height - r2->height))) / 3;
}

// calculate a distance measure between two regions, considering positions only
realno RegionTracker::region_distance(const Region *r1,
				      const Region *r2,
				      int *res_hdist,
				      int *res_vdist)
{
    // calculate horizontal and vertical distances
    int hdistance, vdistance;
    
    if (((r2->xlo >= r1->xlo) && (r2->xlo <= r1->xhi)) ||
	((r1->xlo >= r2->xlo) && (r1->xlo <= r2->xhi)) ||
	((r2->xhi >= r1->xlo) && (r2->xhi <= r1->xhi)) ||
	((r1->xhi >= r2->xlo) && (r1->xhi <= r2->xhi)))
	hdistance = 0;
    else
	hdistance = min(abs((r1->xhi)-(r2->xlo)), abs((r1->xlo)-(r2->xhi)));
    
    if (((r2->ylo >= r1->ylo) && (r2->ylo <= r1->yhi)) ||
	((r1->ylo >= r2->ylo) && (r1->ylo <= r2->yhi)) ||
	((r2->yhi >= r1->ylo) && (r2->yhi <= r1->yhi)) ||
	((r1->yhi >= r2->ylo) && (r1->yhi <= r2->yhi)))
	vdistance = 0;
    else
	vdistance = min(abs((r1->yhi)-(r2->ylo)), abs((r1->ylo)-(r2->yhi)));
    
    if (res_hdist != NULL)  // somebody wants this value?
	*res_hdist = hdistance;
    
    if (res_vdist != NULL)  // somebody wants this value?
	*res_vdist = vdistance;
    
    return hypot((realno)hdistance, (realno)vdistance);
}

// This should be called after all other trackers to correct tracks
//   (try matching against results from ActiveShapeTracker) and clean away
//   tracks or hypotheses which could not be matched
void RegionTracker::post_process_frame(Inputs *inputs,
				       Results *results)
{
    TrackedObjectSet *objects = results->get_tracked_objects();
    int index1;   // index into the TrackedObjectSet *objects
    frame_id_t current_frame_id = results->get_motion_image()->get_frame_id();
    
    //
    // 1 - look in all objects for non-matched regions where a profile was tracked
    //
    for (index1 = 0; index1 < objects->no_items; index1++)
    {
	TrackedObject *obj1 = (*objects)[index1];
	
	// make sure obj1 has regions
	if (obj1->regions->no_items == 0)
	    continue;
	
	assert (obj1->regions->no_items == 1); // FIXME: other cases to be implemented
	
	// make sure obj1 has exactly one profile
	if (obj1->profiles->no_items != 1)          // FIXME (TODO): handle case >1
	    continue;
	
	Region *region = obj1->regions->first->dat;
	Profile *profile = obj1->profiles->first->dat;
	
	// do not match region if it has been matched to or is a measurement
	if (region->source == MEASUREMENT)
	    continue;
	
	// match to a profile only if it is a measurement
	if (profile->source != MEASUREMENT)
	    continue;
	
	cdebug << "RT: RegionTracker::correct_and_cleanup_tracks(): correcting unmatched region... "
	       << endl;
	
	// check whether the remainder of the region could make another person  // FIXME: check height as well as width!!!
	if ((region->width > 1.7 * profile->width) &&
	    (max(profile->xlo - region->xlo,
		 region->xhi - profile->xhi) > 0.85 * profile->width))
	{
	    // assume these are two people  // FIXME: or three???
	    Region *remainder;
	    
	    // check whether the remainder is left or right of our profile
	    if (abs(profile->xlo - region->xlo) < abs(region->xhi - profile->xhi))
	    {
		// create remainder in right part of region
		remainder = new Region(max(max(profile->xhi, (int) region->origin.x),
					   (int) (region->xhi - 1.5 * profile->width)),
				       region->xhi,
				       profile->ylo, profile->yhi);  // FIXME: assuming same height
	    }
	    else
	    {
		// create remainder in left part of region
		remainder = new Region(region->xlo,
				       min(min(profile->xlo, (int) region->origin.x),
					   (int) (region->xlo + 1.5 * profile->width)),
				       profile->ylo, profile->yhi);  // FIXME: assuming same height
	    }
	    TrackedObject *r_object = objects->add_observation(remainder,
							       region->frame_last_detected,
							       region->time_last_detected);
	    
	    // correct status set by TrackedObjectSet::add_observation
	    remainder->source = region->source;  // ie MEASUREMENT
	    remainder->frame_first_detected = region->frame_first_detected;
	    remainder->time_first_detected = region->time_first_detected;
	    //leave at true for now     remainder->is_visible = false;
	}
	
	// now correct position, size and status of the region matched to the profile
	region->Observation::operator=(*profile);
	region->old_origin = region->origin;
	region->direction.x = region->direction.y = 0;
    }


    //	
    // 2 - clean away unmatched regions which were created by splitting
    //
    for (index1 = 0; index1 < objects->no_items; index1++)
    {
	TrackedObject *obj1 = (*objects)[index1];
	
	// make sure obj1 has regions
	if (obj1->regions->no_items == 0)
	    continue;
	
	assert (obj1->regions->no_items == 1);  // cannot handle more than one for now	
	
	if (obj1->regions->first->dat->source == SPLITTING)
	{
// FIXME?	    assert (obj1->profiles->no_items == 0);  // otherwise we're in trouble
	    
	    // synthetic object not matched: remove region so object will be cleaned up later
	    obj1->regions->destroy_all();
	}
    }

    //	
    // 3 - clean away new objects which are contained completely within a
    //     profile (noise in the motion image!)
    //
    for (index1 = 0; index1 < objects->no_items; index1++)
    {
	TrackedObject *obj1 = (*objects)[index1];

	// make sure obj1 has regions
	if (obj1->regions->no_items == 0)
	    continue;
	
	ListNode<Region> *reg1;
	bool reg1_was_destroyed;
	
	// for each region: check if it is new and within some profile
	for (reg1 = obj1->regions->first; reg1 != NULL; )  // increment done below
	{
	    // only check new regions
	    if (reg1->dat->frame_first_detected != current_frame_id)
	    {
		reg1 = reg1->next;
		continue;
	    }
	    
	    // do not check regions which are integrated into the background
	    assert(reg1->dat->incorporated_into_background == false);  // otherwise something is wrong

	    reg1_was_destroyed = false;

	    ListNode<TrackedObject> *obj2;
	    
	    // now check all profiles in all objects
	    for (obj2 = objects->first; (obj2 != NULL) && (reg1_was_destroyed == false);
		 obj2 = obj2->next)
	    {
		ListNode<Profile> *prf2;
		
		for (prf2 = obj2->dat->profiles->first; prf2 != NULL; prf2 = prf2->next)
		{
		    // if profile prf2->dat contains our region reg1->dat, destroy region

		    // only check against visible regions
		    if (prf2->dat->source != MEASUREMENT)
			continue;
	   
		    assert(prf2->dat->frame_last_detected == current_frame_id);  // otherwise something is wrong
		    
		    // now check bounding boxes
		    if ((prf2->dat->xlo >= reg1->dat->xlo) || 
			(prf2->dat->xhi <= reg1->dat->xhi) || 
			(prf2->dat->ylo >= reg1->dat->ylo) || 
			(prf2->dat->yhi <= reg1->dat->yhi))
			continue;		    

		    // the region is completely within the profile, so destroy it
		    ListNode<Region> *next_reg1 = reg1->next;
		    obj1->regions->destroy(reg1);
		    reg1 = next_reg1;
		    
		    reg1_was_destroyed = true;
		}
	    }

	    if (reg1_was_destroyed == false)
		reg1 = reg1->next;	
	}
    }
}

} // namespace ReadingPeopleTracker