activeshapetracker.cc

不错的shijuegezong的程序

	    
	    // ignore background objects here
	    if ((obj2->regions->no_items > 0) &&
		(obj2->regions->first->dat->incorporated_into_background))
		continue;
	    
	    prf2 = obj2->profiles->first->dat;
	    
	    // do the actual comparison: if the match, disable the worse fit
	    compare_two_and_disable_bad_profile(prf1, prf2);
	    
	    // move over tracking data to older track from disabled profile
	    // NB assumes lower id for older objects (should be ok)
	    if ((prf1->fitness <= 0) && (obj1->id < obj2->id))
	    {
		Profile *tmp_prf = prf1;
		prf1 = prf2;
		prf2 = tmp_prf;
	    }
	    else 
		if ((prf2->fitness <= 0) && (obj2->id < obj1->id))
		{
		    Profile *tmp_prf = prf1;
		    prf1 = prf2;
		    prf2 = tmp_prf;
		}
	}
    }
    
    // after marking profiles for deletion destroy all these and other bad profiles
    for (index1 = 0; index1 < objects->no_items; index1++)  // look in all objects:
    {
	obj1 = (*objects)[index1];
	
	for (index2 = 0; index2 < obj1->profiles->no_items; )  // all profiles in object:
	{
	    prf2 = (*obj1->profiles)[index2];
	    
	    if (prf2->fitness <= 0)
	    {
		obj1->profiles->destroy(prf2);
		cdebug << "ActiveShapeTracker::post_process_frame() 3: cleaned away bad profile in object "
		       << obj1->id << " " << endl;
		
		// NB: The TrackedObject will be cleaned away elsewhere if it is empty now
	    }
	    else
		index2++;
	}
    }
}

void ActiveShapeTracker::compare_two_and_disable_bad_profile(Profile *prf1, Profile *prf2)
{    
//      const realno ox1 = (prf1->xlo + prf1->xhi) /2;
//      const realno oy1 = (prf1->ylo + prf1->yhi) /2;
//      const realno w1 =  prf1->width;
//      const realno h1 =  prf1->height;
//
//      const realno ox2 = (prf2->xlo + prf2->xhi) /2;
//      const realno oy2 = (prf2->ylo + prf2->yhi) /2;
//      const realno max_delta_x = fmax(w1,prf2->width) / 3;  // max deviation in x
//      const realno max_delta_y = fmax(h1,prf2->height) / 3; // max deviation in y
//    
//      // first, check whether prf1 and prf2 are at the same position in the image
//      if (! ((ox2 > ox1 - max_delta_x) && (ox2 < ox1 + max_delta_x) &&
//  	   (oy2 > oy1 - max_delta_y) && (oy2 < oy1 + max_delta_y)))
//  	return;
    
    const realno ox1 = prf1->origin.x;
    const realno oy1 = prf1->origin.y;
    const realno ox2 = prf2->origin.x;
    const realno oy2 = prf2->origin.y;
    
    // TODO:  CHECK CHECKS
    
    // first, check whether prf1 and prf2 are at the same position in the image
    if (! (((ox2 > prf1->xlo) && (ox2 < prf1->xhi) &&
	    (oy2 > prf1->ylo) && (oy2 < prf1->yhi)) ||
	   ((ox1 > prf2->xlo) && (ox1 < prf2->xhi) &&
	    (oy1 > prf2->ylo) && (oy1 < prf2->yhi))))
  	return;
    
    // TODO:  CHECK DIRECTION HERE (crossing people!) !!!!
   
    // NB assuming that size is already checked (people at the same position
    //     in to image have around the same pixel size so if they had not one
    //     of them had a bad fit and would be removed)
    
    if (prf2->fitness + 0.05 < prf1->fitness)  // prf1 clearly fits better
    {
	prf2->fitness = 0;  // mark for deletion

	prf1->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf1->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
	return;
    }
    
    if (prf1->fitness + 0.05 < prf2->fitness)  // prf2 clearly fits better
    {
	prf1->fitness = 0;  // mark for deletion

	prf2->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf2->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
	return;
    }
    
    // shapes are similarly good: keep the older one
    if (prf2->frame_first_detected + 3 < prf1->frame_first_detected)  // prf1 is much older
    {
	prf2->fitness = 0;  // mark for deletion

	prf1->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf1->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
	return;
    }
    
    if (prf1->frame_first_detected + 3 < prf2->frame_first_detected)  // prf2 is much older
    {
	prf1->fitness = 0;  // mark for deletion

	prf2->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf2->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
	return;
    }
    
    // shapes are similarly good and old : keep the one with the lowest positional variance
    if (prf1->track_error > prf2->track_error)
    {
	prf1->fitness = 0;  // mark for deletion

	prf2->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf2->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
    }
    else
    {
	prf2->fitness = 0;  // mark for deletion

	prf1->frame_first_detected = min(prf1->frame_first_detected,
					 prf2->frame_first_detected);
	prf1->time_first_detected = min(prf1->time_first_detected,
					prf2->time_first_detected);
    }
}

void ActiveShapeTracker::initialise_track(Profile *curr_prf)
{
    realno init_scale = curr_prf->height / active_model->mean->height;
    realno init_theta = 0.0;
    curr_prf->a_x = init_scale * cos(init_theta);
    curr_prf->a_y = init_scale * sin(init_theta);
    active_model->set_profile(curr_prf);
    active_model->setup_shape_filters();
    active_model->b_to_x();
    curr_prf->get_size();
    
    curr_prf->pos_filter = 
	new DynamicKalmanTwoD(SQUARE(noise_vel_sd * curr_prf->height), 
			      SQUARE(noise_vel_sd * curr_prf->height),
			      SQUARE(noise_acc_sd * curr_prf->height),
			      SQUARE(noise_acc_sd * curr_prf->height),
			      curr_prf->origin,
			      Point2(0,0),
			      SQUARE(init_pos_sd * curr_prf->height),
			      SQUARE(init_pos_sd * curr_prf->height),
			      SQUARE(init_vel_sd * curr_prf->height),
			      SQUARE(init_vel_sd * curr_prf->height), 1.0);
    
    
    
    curr_prf->a_filter = new StaticKalmanTwoD(0, 0, 0, curr_prf->a_x, 
					      curr_prf->a_y, 0, 0, 0);
    curr_prf->a_filter->set_polar_noise_cov(SQUARE(init_scale_sd), 
					    SQUARE(init_theta_sd));
    curr_prf->a_filter->update_state();
    curr_prf->a_filter->set_polar_noise_cov(SQUARE(noise_scale_sd),
					    SQUARE(noise_theta_sd));
    
    // remember that filters are initialised
    curr_prf->filters_initialised = true;    
    
    
    
    
    
#ifndef NO_DISPLAY
#ifdef DEBUG
    if (debug_level == 3) 
    {
//	inputs->get_video_image()->draw_in_image();
	active_model->set_profile(curr_prf);
	curr_prf->update_shape_filters();
	active_model->apply_virtual_shape();
	active_model->b_to_x();
	
	if (getplanes() == 8)   // CMap (grey?) image  (fi motion image)
	    color(127);
	else
	    if (getplanes() == 24)          // RGB mode  (fi video image)
		RGBcolor(255,255,255);  // use white for initialisations
	
	curr_prf->draw();
	gflush();
	sleep(1);
    }    
    
#endif
#endif
    
}

// tracking loop for each new image
void ActiveShapeTracker::process_frame(Inputs *inputs,
				       Results *results,
				       unsigned int max_objects)
{
    inputs->get_video_image()->draw_in_image();
    
#ifndef NO_DISPLAY
#ifdef DEBUG
    if (debug_level == 3) 
    {
	inputs->get_video_image()->display();
	ListNode<TrackedObject> *curr_obj;
	
	TrackedObjectSet *objects = results->get_tracked_objects();
	
	// get old profiles from tracked object set, predicting each
	for (curr_obj = objects->first; curr_obj != NULL; curr_obj = curr_obj->next)	
	{
	    if (getplanes() == 8)   // CMap (grey?) image  (fi motion image)
		color(127);
	    else
		if (getplanes() == 24)      // RGB mode  (fi video image)
		    Image::set_colour(curr_obj->dat->id);  // set colour according to id
	    
	    curr_obj->dat->regions->draw_boxes();
	    curr_obj->dat->features->draw();
	}
    }
#endif
#endif
    
    // set images for use in ActiveModel and its EdgeDetector
    active_model->set_video_image(inputs->get_video_image());
    active_model->set_difference_image(results->get_difference_image());  // could be NULL.  OK.
    active_model->set_background_image(results->get_background_image());
    active_model->set_previous_video_image(previous_video_image);  // could be NULL.  OK.
    
    results->get_occlusion_image()->clear(CLEAR_MARK);  // FIXME: should we?
    cdebug << " >>> ActiveShapeTracker::process_frame: "
	   << " results->get_occlusion_image()->clear(CLEAR_MARK); --- (data at "
	   << (void*) results->get_occlusion_image()->get_data() << ") <<< "  // FIXME: should we?
	   << endl;
    
    predict_old_objects(results);
    
    track_old_objects(inputs, results);
    
    detect_new_objects(inputs, results);
    
    // maybe we use a MovingEdgeDetector, which needs the previous_video_image
    //   save current image for that purpose in the next frame
    if (previous_video_image != NULL)
	inputs->get_video_image()->copy(previous_video_image);
    
#ifndef NO_DISPLAY
#ifdef DEBUG
    if ((debug_level == 3) && (results->get_tracked_objects()->no_items > 0))
    {
	sleep(5);
    }
#endif
#endif

}

EdgeDetector *ActiveShapeTracker::setup_edge_detector(Inputs *inputs,
						      MotionDetector *motion_detector,
						      char *detection_method)
{
    bool use_background_image = false;
    bool use_difference_image = false;   // whether to use a pre-calculated diff
    
    // do we need a full size background image and a difference image for edge detection?
    if (strstr(detection_method, "FOREGROUND")) 
	use_background_image = use_difference_image = true;
    
    EdgeDetector *edge_detector = NULL;
    
    if (strstr(detection_method, "COLOUR") && 
	(inputs->get_video_image()->get_image_type() != RGB32))
    {
	cerror << " ActiveShapeTracker::setup_edge_detector(): ignoring colour edge detection "
	       << "for non-colour image" << endl;
	detection_method = "FOREGROUND_EDGE";
    }    
    
    if (strcmp(detection_method, "COLOUR_FOREGROUND_EDGE") == 0)
    {
	edge_detector =
	    new ColourForegroundEdgeDetector
	    (edge_threshold,
	     edge_threshold, this);
    }
    else
	if (strcmp(detection_method,"MOVING_EDGE") == 0)
	{
	    // MovingEdgeDetector needs the previous video image.  provide it...
	    // FIXME: assuming RGB32Image.  Problems would be detected in Grey8Image::copy() later.
	    previous_video_image = new RGB32Image(inputs->get_video_image_source()->get_xdim(),
						  inputs->get_video_image_source()->get_ydim());

	    edge_detector =
		new MovingEdgeDetector(edge_threshold, STATISTICAL_EDGE_SEARCH, this);
	}
	else
	    if (strcmp(detection_method,"SIMPLE_EDGE") == 0)
		edge_detector =
		    new SimpleEdgeDetector(edge_threshold,STATISTICAL_EDGE_SEARCH, this);
	    else
		if (strcmp(detection_method,"SOBEL_EDGE") == 0)
		    edge_detector =
			new SobelEdgeDetector(edge_threshold, STATISTICAL_EDGE_SEARCH, this);
		else
		    if (strcmp(detection_method,"FOREGROUND_EDGE") == 0)
			edge_detector =
			    new ForegroundEdgeDetector(edge_threshold, edge_threshold, 
						       MAXIMUM_EDGE_SEARCH, this);
		    else
			if (strcmp(detection_method,"NORMALISED_COLOUR_FOREGROUND_EDGE") == 0)
			    edge_detector =
				new NormalisedColourForegroundEdgeDetector(edge_threshold,
									   edge_threshold, this);
    
    if (edge_detector == NULL)
    {
	cerror << "ActiveShapeTracker::setup_edge_detector(): no valid edge detection "
	       << "method given. " << endl
	       << " trying a simple ForegroundEdgeDetector " << endl;
	edge_detector =
	    new ForegroundEdgeDetector(edge_threshold, edge_threshold, 
				       MAXIMUM_EDGE_SEARCH, this);
    }
    
    return edge_detector;
}

// use Calibration class to create additional hypotheses about person heights
void ActiveShapeTracker::create_hypotheses_using_calibration(Profile *profile,
							     ProfileSet *results)
{
    // image vectors in NAG vector format, homogeneous:
    NagVector origin(3);    // origin (centre) of the person
    NagVector feet(3);      // lowest image point of the person
    NagVector head(3);      // highest image point of the person

    // determine the three positions
    origin[0] = profile->origin.x;
    origin[1] = profile->origin.y;
    origin[2] = 1;

    feet = origin;
    feet[1] = profile->ylo;
    
    head = origin;
    head[1] = profile->yhi;
    
    assert(fabs(profile->height - (profile->yhi - profile->ylo)) < 1.5);
    
    // <= because in extreme cases, the height might be 0...
    assert((Image::image_addressing_mode == IA_TOP_TO_BOTTOM) ^ (feet[1] <= head[1]));

    // convert min/max/typical person height from world to image coordinates at origin
    realno min_image_height = calibration->get_image_distance_from_height_in_cm
	(origin, MIN_WORLD_HEIGHT, MIN_WORLD_HEIGHT / 2);          // height: centre
    
    realno max_image_height = calibration->get_image_distance_from_height_in_cm
	(origin, MAX_WORLD_HEIGHT, MAX_WORLD_HEIGHT / 2);          // height: centre
    
    realno typical_image_height = calibration->get_image_distance_from_height_in_cm
	(origin, TYPICAL_WORLD_HEIGHT, TYPICAL_WORLD_HEIGHT / 2);  // height: centre
    
    // check given profile for its size
    realno world_height = calibration->get_world_distance_from_height_in_pixels
	(feet, profile->height, 0);
    
    if ((profile->height < min_image_height) || (profile->height > max_image_height))
    {
	// too large or too small.  Modify height
	cdebug << "ActiveShapeTracker::create_hypotheses_using_calibration(): "
	       << "height check: " << profile->height << " pix == "
	       << world_height << " cm -> not a person." << endl;
	
	// create and add hypothesis 1: we see the feet of the person
	Profile *new_profile = new Profile (profile->origin.x,
					    profile->ylo + typical_image_height / 2,
					    profile->width,
					    typical_image_height);
	// adjust status
	new_profile->source = profile->source;      // e.g. MEASUREMENT
	
	// add to results
	results->add(new_profile);
	
	// create and add hypothesis 2: we see the head of the person
	new_profile = new Profile (profile->origin.x,
				   profile->ylo + typical_image_height / 2,
				   profile->width,
				   typical_image_height);
	
	// adjust status
	new_profile->source = profile->source;      // e.g. MEASUREMENT
	
	// add to results
	results->add(new_profile);
	
	// if requested, remove original profile from results
	if (new_hypothesis_replaces_measurement == true)
	    results->destroy(profile);
	
	cdebug << "ActiveShapeTracker::create_hypotheses_using_calibration(): "
	       << "changed to " << typical_image_height << " pix, that's " 
	       << calibration->get_world_distance_from_height_in_pixels
	    (origin, TYPICAL_WORLD_HEIGHT, TYPICAL_WORLD_HEIGHT / 2) << " cm (goal: " 
	       << TYPICAL_WORLD_HEIGHT << " cm) " << endl;
	
    }
    else
	cdebug << "ActiveShapeTracker::create_hypotheses_using_calibration(): "
	       << "height check: " << profile->height << " pix == "
	       << world_height << " cm -> OK." << endl;
    
    // TODO: could to more neat stuff here like making more guesses
}

} // namespace ReadingPeopleTracker