activeshapetracker.cc

不错的shijuegezong的程序

/***************************************************************
 * C++ source
 *
 * File : 	ActiveShapeTracker.cc
 *
 * Module :	ActiveShapeTracker
 *
 * Author : 	A M Baumberg (CoMIR)
 *
 * Creation Date : Tue Oct 22 15:06:09 1996 
 *
 * Comments : 	High level people tracker
 *
 ***************************************************************/


// includes

#include <cassert>

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

#include "ActiveShapeTracker.h"
#include "TrackedObjectSet.h"
#include "EdgeDetector.h"
#include "Image.h"
#include "Calibration.h"
#include "Inputs.h"
#include "ScreenOutput.h"
#include "text_output.h" // for cerror etc
#include "ActiveModel.h"
#include "Results.h"
#include "MotionDetector.h"
#include "text_output.h"
#include "PeopleTracker.h"
#include "OcclusionImage.h"

namespace ReadingPeopleTracker
{

// BaseTracker::register_base_configuration_variables() registeres some for us!
void ActiveShapeTracker::register_configuration_parameters()
{
    cloud_size = 
	configuration.register_real("PCA_CLOUD_SIZE", 2.0,
				    &cloud_size,
				    false, "ActiveModel", 
				    "The maximum Mahalanobis distance to the \
mean shape (in std deviations)");
    
    noise_scalar = 
	configuration.register_real("SHAPE_NOISE_SD", sqrt(0.2),
				    &noise_scalar,
				    false, "ActiveModel",
				    "The noise term for each shape parameter \
in std deviations per frame");

    initial_noise =
	configuration.register_real("INIT_SHAPE_SD", 1.0,
				    &initial_noise,
				    false, "ActiveModel",
				    "The initial uncertainty of each shape \
parameter in std deviations");
    
    measure_fac_sd =
	configuration.register_real("MEASURE_FAC_SD", 0.1,
				    &measure_fac_sd,
				    false, "ActiveModel",
				    "The standard deviation of measurements \
proportional to the search window size");
    
    measure_const_sd =
	configuration.register_real("MEASURE_CONST_SD", 0.5,
				    &measure_const_sd,
				    false, "ActiveModel",
				    "An additional constant value added to the \
std deviation of the measurement process (in pixels)");
    
    default_subdivisions =
	configuration.register_int("SPLINE_SUBDIVISIONS", 8,
				   &default_subdivisions,
				   true, "ActiveModel",
				   "The number of sample points between spline \
control points");
    
    default_model_depth = 
	configuration.register_int("MODEL_DEPTH", 10,
				   &default_model_depth,
				   true, "ActiveModel",
				   "The number of shape parameters used");
    
    bunch_size =
	configuration.register_int("BUNCH_SIZE", 2,
				   &bunch_size,
				   false, "ActiveModel",
				   "The number of measurement made \
per free parameter");
    
    
    use_hierarchical_search = 
	configuration.register_bool("HIERARCHICAL", true,
				    &use_hierarchical_search,
				    false, "ActiveModel",
				    "Vary the number of shape paramaters and \
measurements as the fitting process converges");
    
    do_m_search =
	configuration.register_bool("MAHALANOBIS_DIRECTION", true,
				    &do_m_search,
				    false, "ActiveModel",
				    "Search along the Mahalanobis optimal search \
direction");
    
    minimum_best_gain = 
	configuration.register_real("MIN_GAIN", 0.5,
				    0.0, 1.0,
				    &minimum_best_gain,
				    false, "ActiveModel",
				    "The desired initial gain and the minimum \
subsequent gain for parameter fitting");
    
//////////////////////////////////////////////////
    
    FITNESS_THRESHOLD  =
	configuration.register_real("START_FITNESS_THRESHOLD", 0.85,
				    0.0, 1.0,
				    &FITNESS_THRESHOLD,
				    false, "ActiveShapeTracker",
				    "Minimum fitness score to initialise track");
    
    VERY_POOR_FIT =
	configuration.register_real("TRACK_FITNESS_THRESHOLD", 0.4,
				    0.0, 1.0,
				    &VERY_POOR_FIT,
				    false, "ActiveShapeTracker",
				    "Minimum fitness score to maintain track");
    
    SMALL_NO_FRAMES =
	configuration.register_int("SMALL_NO_FRAMES", 3,
				   &SMALL_NO_FRAMES,
				   false, "ActiveShapeTracker",
				   "Number of frames before track is accepted");
    
    MIN_SIGNIFICANCE =
	configuration.register_real("MIN_SIGNIFICANCE", 0.3,
				    0.0, 1.0,
				    &MIN_SIGNIFICANCE,
				    false, "ActiveShapeTracker",
				    "Fraction of curve that must be visible for tracking");
    
    terminate_thresh =
	configuration.register_real("TERMINATION_THRESHOLD", 100,
				    &terminate_thresh,
				    false, "ActiveShapeTracker",
				    "Maximum SD of ground plane position (in cm)");
    
    init_scale_sd =
	configuration.register_real("INIT_SCALE_SD", 0.015,
				    &init_scale_sd,
				    false, "ActiveShapeTracker",
				    "SD of initial estimate of object size \
relative to initial estimate");
    
    noise_scale_sd =
	configuration.register_real("NOISE_SCALE_SD", 0.05,
				    &noise_scale_sd,
				    false, "ActiveShapeTracker",
				    "SD of scale noise term relative to initial scale");
    
    TEMPORAL_SKIP =
	configuration.register_int("FRAME_SKIP", 1,
				   &TEMPORAL_SKIP,
				   true, "ActiveShapeTracker",
				   "Temporal resampling for object detection (1==no skip)");
    
    edge_threshold =
	configuration.register_real("EDGE_THRESHOLD", 0.05,
				    0.0, 1.0, &edge_threshold,
				    false, "ActiveShapeTracker",
				    "A significance threshold for edge detection");
    
    edge_detection_method =
	configuration.register_string("EDGE_DETECTION", "COLOUR_FOREGROUND_EDGE",
				      &edge_detection_method, true,
				      "ActiveShapeTracker",
				      "The type of edges used for tracking",
				      EdgeDetector::edge_detection_method_names);
    
    init_pos_sd =
	configuration.register_real("INIT_POS_SD", 0.1,
				    &init_pos_sd,
				    false, "ActiveShapeTracker",
				    "Initial standard deviation of position\
 relative to object height");
    
    init_vel_sd =
	configuration.register_real("INIT_VEL_SD", 0.1,
				    &init_vel_sd,
				    false, "ActiveShapeTracker",
				    "Initial standard deviation of velocity\
 relative to object height");
    
    noise_vel_sd =
	configuration.register_real("REL_VEL_SD", 0.1,
				    &noise_vel_sd,
				    false, "ActiveShapeTracker",
				    "standard deviation of random velocity\
 term relative to object height");

    noise_acc_sd =
	configuration.register_real("REL_ACC_SD", 0.1,
				    &noise_acc_sd,
				    false, "ActiveShapeTracker",
				    "standard deviation of random acceleration\
 term relative to object height");    
    
    init_theta_sd =
	configuration.register_real("INIT_THETA_SD", 0.015,
				    &init_theta_sd,
				    false, "ActiveShapeTracker",
				    "Standard deviation of initial estimate of\
 rotation angle");
    
    noise_theta_sd =
	configuration.register_real("NOISE_THETA_SD", 0.0,
				    &noise_theta_sd,
				    false, "ActiveShapeTracker",
				    "Standard deviation of theta noise term");    
    
    pca_model_filename =
	configuration.register_string("PCA_MODEL_FILENAME",
				      NULL, &pca_model_filename, true,
				      "ActiveShapeTracker",
				      "An optional eigenshape model file.  If \
not set, the default model is used.");
    
    new_hypothesis_replaces_measurement
	= configuration.register_bool("NEW_HYPOTHESIS_REPLACES_MEASUREMENT", true,
				    &new_hypothesis_replaces_measurement,
				    false, "ActiveShapeTracker",
				    "whether to remove the original measurement when creating new hypotheses using calibration");
   
}//end of register_configuration_parameters()


ActiveShapeTracker::ActiveShapeTracker(Inputs *inputs,
				       MotionDetector *motion_detector,
				       char *config_filename)
{
    calibration = inputs->get_calibration();
    
    if (calibration != NULL)
	use_calibration = true;
    else
	use_calibration = false;
    
    if (config_filename == NULL)
    {
	cerror << "ActiveShapeTracker: cannot instantiate without configuration file "
	       << endl;
	exit(1);
    }
    
    //member function registers appropriate configuration parameters with configuration manager
    register_configuration_parameters();
    
    //read from configuration file storing vales in correct place using configuration manager
    configuration.parse_parameter_file(config_filename);
    
    previous_video_image = NULL;

    // set up active model (before setting up the edge detector)
    active_model = new ActiveModel(this, inputs->get_video_image_source()->get_image_type());

    // set up edge detector using the ShapeTracker configuration var edge_detection_method
    edge_detector = setup_edge_detector(inputs, motion_detector, edge_detection_method);
    
    active_model->set_edge_detector(edge_detector);
    
    cdebug << " >>>  ActiveShapeTracker::ActiveShapeTracker: active_model->set_video_image(inputs->get_video_image());  <<< " << endl;
    
    active_model->set_occlusion_handler (
	new OcclusionImage((Grey8Image *)motion_detector->get_inverted_additional_motion_mask()));
    
    cdebug << " >>>  ActiveShapeTracker::ActiveShapeTracker: active_model->set_occlusion_handler(tracking->get_occlusion_image()); changed to new OcclusionImage(motion_detector->get_inverted_additional_motion_mask()) <<< "
	   << endl;
}


void ActiveShapeTracker::predict_old_objects(Results *results)
{
    // update Kalman predictions (position) for tracked `Profile's.
    // new: we now also get new `Profile's which are not tracked yet (ie no filters)
    //      so there cannot be predictions.  These will just be ignored.
    Profile *curr_prf;
    
//     frame_id_t frame_id = results->get_motion_image()->get_frame_id();
    
    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)	
    {
	// get profile(s) for this tracked object (usually only one)
	ProfileSet *profiles = curr_obj->dat->profiles;
	
	for (profiles->start(); profiles->current_ok(); profiles->forward())
	{
	    // predict profile
	    curr_prf = profiles->get_current();
	    
	    // first check whether the surrounding region is incorporated into the background
	    if ((curr_obj->dat->regions->no_items > 0) &&
		(curr_obj->dat->regions->first->dat->incorporated_into_background))
	    {
		// no prediction, just change status...
		curr_prf->source = PREDICTION;
		curr_prf->is_visible = true;
		// curr_prf->frame_last_detected = frame_id - 1;
		continue;
	    }
	    
	    if (curr_prf->filters_initialised == false)
	    {
		// FIXME: we should not be here.  we are here because of the problem in Profile::operator=, quod vide.
		cdebug << "ActiveShapeTracker::predict_old_objects(): "
		       << " object id " << curr_obj->dat->id
		       << " Warning: curr->dat->filters_initialised == false"
		       << endl;
		
		initialise_track(curr_prf);
		
//  		continue;  // ignore uninitialised Profile (from splitting?)
//		// the Profile will be initialised/tracked in detect_new_objects()
	    }
	    
	    active_model->set_profile(curr_prf);
	    
	    assert(curr_prf->pos_filter != NULL);  // cannot predict uninitialised Profile
	    
	    curr_prf->update_shape_filters();
	    active_model->apply_virtual_shape();
	    
	    DynamicKalmanTwoD *origin_filter =
		(DynamicKalmanTwoD*) curr_prf->pos_filter;
	    
	    origin_filter->
		reset_noise_covariance(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));
	    
	    Point2 origin = origin_filter->get_prediction();
	    curr_prf->origin = origin;
	    
	    curr_prf->a_filter->set_polar_noise_cov(SQUARE(noise_scale_sd),
						    SQUARE(noise_theta_sd));
	    curr_prf->a_filter->update_state();
	    
	    // mark profile as predicted
	    curr_prf->source = PREDICTION;
	    
	    active_model->b_to_x();
	}
    }
}


void ActiveShapeTracker::track_old_objects(Inputs *inputs, Results *results)
{
    // new: we now also get new `Profile's which are not tracked yet
    //      so they do not have valid shape nor filters
    
#ifdef DEBUG
    inputs->get_video_image()->draw_in_image();
#endif
    
    // get frame id and time from  video image
    frame_id_t frame_id = inputs->get_frame_id();
    frame_time_t frame_time = inputs->get_frame_time_in_ms();
    
    ListNode<TrackedObject> *curr_obj;
    
    TrackedObjectSet *objects = results->get_tracked_objects();
    
    // get old, predicted profiles from tracked object set, tracking each
    for (curr_obj = objects->first; curr_obj != NULL; curr_obj = curr_obj->next)	
    {
	// get profile(s) for this tracked object (usually only one)
	ProfileSet *profiles = curr_obj->dat->profiles;
	
	for (ListNode<Profile> *curr = profiles->first; curr != NULL; )
	{
	    // first check whether the surrounding region is incorporated into the background
	    if ((curr_obj->dat->regions->no_items > 0) &&
		(curr_obj->dat->regions->first->dat->incorporated_into_background))
	    {
		// ignore profile (no tracking)
		curr = curr->next;
		continue;
	    }
	    
	    if (curr->dat->filters_initialised == false)
	    {
		cdebug << "ActiveShapeTracker::track_old_objects(): "
		       << " object id " << curr_obj->dat->id
		       << " Warning: curr->dat->filters_initialised == false"
		       << endl;
		
		initialise_track(curr->dat);
		// the Profile will be tracked in detect_new_objects()
	    }
	    
	    cdebug << "ActiveShapeTracker::track_old_objects(): Tracking profile in"
		   << " object id " << curr_obj->dat->id << endl;
	    
	    assert (curr->dat->source == PREDICTION);
	    
	    Profile *curr_prf = curr->dat;
	    
	    // track profile using our ActiveModel
	    active_model->set_profile(curr_prf);
	    
	    assert(curr_prf->pos_filter != NULL);  // cannot predict uninitialised Profile
	    
	    curr_prf->fitness = active_model->track_profile(curr_obj->dat->id);
	    
	    curr_prf->track_error =
		sqrt(curr_prf->pos_filter->get_uncertainty());
	    
	    // update variables like xlo, xhi, width etc
	    curr_prf->update_size_variables();
	    
	    cdebug << "AST: positional variance " << curr_prf->track_error << endl
		   << "AST: observed " << active_model->no_observed
		   << " out of unoccluded " << active_model->no_looks
		   << " out of " << active_model->no_sampled
		   << endl	    
		   << "AST: fitness " << curr_prf->fitness << endl;
	    
	    // check whether track is ok (fit enough to keep)
	    bool is_ok = true;