profile.cc

来自「VC视频对象的跟踪提取原代码（vc视频监控源码）」· CC 代码 · 共 1,063 行 · 第 1/2 页

/*
 *  Profile.cc
 * 
 *  a record class for storing the Profile data
 *  of an object
 *
 *  AMB 27/4/93
 */

#include <math.h>  // for hypot
#include <stdlib.h>

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

#include "Profile.h"
//#include "ActiveShapeTracker.h"
#include "text_output.h" // for cerror etc
#include "SplineWeights.h"
#include "tracker_defines_types_and_helpers.h"

#ifndef NO_DISPLAY
// include graphics libraries...

#ifdef USE_GL
#include <gl.h>

#ifdef LINUX
#ifndef DISPLAY_POLY
// defbasis, curveprecision, curvebasis, crvn are currently missing from Ygl
#ifdef __cplusplus
extern "C"
{
#include <gl.h>
}
#else  // ifdef __cplusplus
#include <gl.h>
#endif // ifdef __cplusplus else
#endif // ifndef DISPLAY_POLY
#endif // ifdef LINUX

#else  // ifdef USE_GL

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <vogl.h>

#endif // ifdef USE_GL else

#endif   // #ifndef NO_DISPLAY

#include "SplineWeights.h"
#include "EnvParameter.h"

#include "Image.h"

#include "EdgeDetector.h"
#include "Calibration.h"

namespace ReadingPeopleTracker
{

// definition and initialisation of static member variables
unsigned int Profile::NO_CONTROL_POINTS = 32;

bool Profile::draw_boxes = true;
bool Profile::draw_arrow = false;
bool Profile::draw_in_colour = true;
bool Profile::draw_rectangle = false;
bool Profile::draw_spline = true;
bool Profile::draw_fill = false;
bool Profile::output_points = true;
bool Profile::draw_dashed = false;
bool Profile::draw_label = true;

int Profile::draw_linewidth = 4;
realno Profile::draw_box_size = 1.5;
const realno Profile::LARGE = 1e10;


Profile::Profile(Point2 &p_o, realno &p_w, realno &p_h, realno&
		 p_grad, PointVector &pnts, Point2 &d) :
    PointVector(NO_CONTROL_POINTS+1),
    Observation(),
    direction(point_data()[NO_CONTROL_POINTS])
{
    spline_weights = NULL;
    origin = p_o;
    width = p_w;
    height = p_h;
    Observation::xlo = real_to_int(origin.x - width / 2);
    Observation::xhi = real_to_int(origin.x + width / 2);
    Observation::ylo = real_to_int(origin.y - height / 2);
    Observation::yhi = real_to_int(origin.y + height / 2);
    inv_grad = p_grad;
    if (pnts.get_data() != NULL)
	pnts.NagVector::copy(*this);
    else
	this->NagVector::clear(0.0);
    direction = d;
    filters = NULL;
    pos_filter = NULL; 
    a_filter = NULL;      
    s_filter = NULL;
    th_filter = NULL;
    colour_info = NULL;
    no_cblobs = 0;
    PCA_flag = 0;
//    gp_filter = NULL;
    a_x = a_y = 0;
    track_error = 0;
    fitness = 0;
    filters_initialised = false;
}

Profile::Profile(realno orig_x, realno orig_y, realno wdth, realno hgt) :
    PointVector(NO_CONTROL_POINTS+1),
    direction(point_data()[NO_CONTROL_POINTS]),
    Observation()
{
    spline_weights = NULL;
    origin.x = orig_x;
    origin.y = orig_y;
    width = wdth;
    height = hgt;
    Observation::xlo = real_to_int(origin.x - width / 2);
    Observation::xhi = real_to_int(origin.x + width / 2);
    Observation::ylo = real_to_int(origin.y - height / 2);
    Observation::yhi = real_to_int(origin.y + height / 2);
    inv_grad = 0;
    direction.x = direction.y = 0.0;
    clear(0);
    filters = NULL;
    pos_filter = NULL; 
    a_filter = NULL;      
    s_filter = NULL;
    th_filter = NULL;
    colour_info = NULL;
    no_cblobs = 0;
    PCA_flag = 0;
//    gp_filter = NULL;
    a_x = a_y = 0;
    track_error = 0;
    fitness = 0;
    filters_initialised = false;
}

Profile::Profile(Profile &original) :
    PointVector(NO_CONTROL_POINTS+1),
    direction(point_data()[NO_CONTROL_POINTS]),
    Observation()
{
    assert (1 == 0);  //  don't use this contructor!   (why would you?)
    
    operator= (original);
}

Profile &Profile::operator=(const Profile &original)
{
    Observation::operator=(original);
    
//    tracker = original.tracker;
    
    inv_grad = original.inv_grad;
    
// FIXME: should really be there but buggy:      filters_initialised = original.filters_initialised;
    cdebug << endl 
	   << " FIXME:  Profile::operator=():  `filters_initialised = original.filters_initialised' commented out "
	   << endl;
    
    filters_initialised = false; ////////////////////  FIXME
    
    for (int i = 0 ; i < NO_CONTROL_POINTS; i++)
	spline[i] = original.spline[i];    

    track_error = original.track_error;
    fitness = original.fitness;
    PCA_flag = original.PCA_flag;
    
    no_shape_parameters = original.no_shape_parameters;
    if (original.filters != NULL)
    {
	filters = new FilterOneD*[no_shape_parameters];
	for (int i = 0; i < no_shape_parameters; i++)
	    filters[i] = original.filters[i]->clone();
    }
    else
	filters = NULL;
    
    a_x = original.a_x;
    a_y = original.a_y;
    if (original.a_filter != NULL)
    {
	a_filter = new StaticKalmanTwoD();
	*a_filter = *original.a_filter;
    }
    else
	a_filter = NULL;
    
    if (original.s_filter != NULL)
    {
	s_filter = new StaticKalmanOneD();
	*s_filter = *original.s_filter;
    }
    else
	s_filter = NULL;
    
    if (original.th_filter != NULL)
    {
	th_filter = new StaticKalmanOneD();
	*th_filter = *original.th_filter;
    }
    else
	th_filter = NULL;
    
    if (original.pos_filter != NULL)
    {      
	pos_filter = (FilterTwoD*) new StaticKalmanTwoD;
	*pos_filter = *original.pos_filter;
    }
    else
	pos_filter = NULL;
    
    no_cblobs = original.no_cblobs;
    
    if (original.colour_info != NULL)
    {
	assert(original.no_cblobs > 0);
	
	colour_info = new ColourBlob[no_cblobs];
	for (int i = 0; i < no_cblobs; i++)
	    colour_info[i].operator=(original.colour_info[i]);
    }
    else
    {
	colour_info = NULL;
    }
    
    return *this;
}

Profile::Profile() : PointVector(NO_CONTROL_POINTS+1),
		     direction(point_data()[NO_CONTROL_POINTS]),
		     Observation()
{
    spline_weights = NULL;
    origin.x = origin.y = width = height = inv_grad = 0; 
    direction.x = direction.y = 0;
    filters = NULL;
    pos_filter = NULL; 
    a_filter = NULL;      
    s_filter = NULL;
    th_filter = NULL;
    colour_info = NULL;
    no_cblobs = 0;
    PCA_flag = 0;
//    gp_filter = NULL;
    a_x = a_y = 0;
    track_error = 0;
    fitness = 0;
    filters_initialised = false;
}

Profile::~Profile()
{
    if (filters != NULL)  delete [] filters;
    if (a_filter != NULL)  delete a_filter;
    if (s_filter != NULL)  delete s_filter;
    if (pos_filter != NULL)  delete pos_filter;
    if (th_filter != NULL)  delete th_filter;
//    if (gp_filter != NULL)  delete gp_filter;
// FIXME: does not work. why?       if (colour_info != NULL)  delete [] colour_info;
}

ostream &operator<< (ostream &out_strm, const Profile &profile)
{
    out_strm << "origin " << profile.origin << endl;
    out_strm << "width " << profile.width << endl;
    out_strm << "height " << profile.height << endl;
    
    if (profile.direction != Point2(0,0))
	out_strm << "direction " << profile.direction << endl;
    
    if (profile.filters_initialised)
    {
// 	// see whether we have calibration data and put out 3D info
// 	if ((profile.tracker != NULL) && (profile.tracker->calibration != NULL))
// 	{
// 	    // new style Calibration class
// 	    NagVector nag_origin(2);
	    
// 	    nag_origin[0] = profile.origin.x;
// 	    nag_origin[1] = profile.ylo;  //  NB: not origin.y as origin is in the centre of the person
	    
// //    	    Image::set_colour(2);	    
// //    	    move2(origin.x,ylo);
// //    	    draw2(origin.x,ylo+height);
// //    	    Image::set_colour(5);
// //    	    move2(origin.x+2,ylo);
// //    	    realno h = tracker->calibration->get_image_distance_from_height_in_cm(nag_origin,170);
// //    	    draw2(origin.x,ylo+h);
// //  	    sleep(2);
	    
// 	    NagVector world_position = profile.tracker->calibration->get_world_from_ip(nag_origin);
// 	    out_strm << "world_pos ("
// 		     << world_position[0] << "," << world_position[1]
// 		     << ") " << endl;
	    
// 	    out_strm << "world_height "
// 		     << profile.tracker->calibration->get_world_distance_from_height_in_pixels(nag_origin,
// 											       profile.height)
// 		     << " " << endl;
// 	}
	
	out_strm << "shape_parameters";
	int i;
	for (i = 0; i <profile. no_shape_parameters; i++)
	    out_strm << "  " << profile.filters[i]->get_state();
	out_strm << endl;
	
	out_strm << "fitness " << profile.fitness << endl;
	out_strm << "positional variance  " << profile.track_error << endl;
    }
    
    if (Profile::output_points)
    {
	out_strm << "control_points ";
	for (int i = 0; i < Profile::NO_CONTROL_POINTS; i++)
	    out_strm << profile.spline[i] << " ";
	out_strm << endl;
    }
    
    return out_strm;
}

// new version of operator>> allows more flexibility
istream &operator>>(istream &in_strm, Profile &pf)
{
    char dummytag[64];
    bool finished = false;
    
    // keep going until we reach a 'control_points' tag
    // OR a second 'label' or 'origin' tag
    
    bool found_label = false;
    bool found_origin = false;
    pf.inv_grad = 0;
    pf.direction = Point2(0,0);
    while (!finished)
    {
	in_strm >> dummytag;
	if (strcmp(dummytag,"label") == 0)
	{
	    if (found_label) 
	    {
		finished = true;
		Profile::putback_string(in_strm, dummytag);
		break;
	    }
	    found_label = true;
//	    in_strm >> pf.ref_no;
	    continue;
	}
	else
	    if (strstr(dummytag,"origin") != NULL) // matches *origin*
	    {
		if (found_origin)
		{
		    finished = true;
		    Profile::putback_string(in_strm,dummytag);
		    break;
		}
		found_origin = true;
		Point2 origin;
		in_strm >> origin;
		pf.origin = origin;
	    }
	    else
		if (strstr(dummytag,"width") != NULL) // matches *width*
		    in_strm >> pf.width;
		else
		    if (strstr(dummytag,"height") != NULL) // matches *height*
			in_strm >> pf.height;
		    else
			if (strstr(dummytag,"direction") != NULL) // matches *direction*
			    in_strm >> pf.direction;
			else
			    if (strstr(dummytag,"control_points") != NULL)
			    {
				for (int i = 0; i < Profile::NO_CONTROL_POINTS; i++)
				    in_strm >> pf.spline[i];
				finished = true;
			    }
	
	// ignore up to end of line 
	in_strm.ignore(999,'\n');
    }
    return in_strm;
}

#ifndef NO_DISPLAY
#ifndef DISPLAY_POLY
NagMatrix bsplinematrix =
{
    { -1.0/6.0,  3.0/6.0, -3.0/6.0, 1.0/6.0 },
    {  3.0/6.0, -6.0/6.0,  3.0/6.0,     0   },
    { -3.0/6.0,      0,    3.0/6.0,     0   },
    {  1.0/6.0,  4.0/6.0,  1.0/6.0,     0   },
};
#endif
#endif   // #ifndef NO_DISPLAY


#ifndef NO_DISPLAY
typedef Coord gl_vec3[3];
typedef Coord gl_vec2[2];
#endif   // #ifndef NO_DISPLAY

void Profile::draw(int col1, int col2, int col3)
{
#ifndef NO_DISPLAY
    int i;
    int j;
//      if (Profile::draw_in_colour)
//  	Image::set_colour(col1);
    
    if (Profile::draw_dashed)
	setlinestyle(2);
    else
	setlinestyle(0);
    
#ifndef DISPLAY_POLY
    defbasis(1,bsplinematrix);
    curveprecision(15);
    curvebasis(1);
#endif
    
//    linewidth(Profile::draw_linewidth);
    
    if ((Profile::draw_spline) && (Profile::draw_fill == false))
    {
#ifdef DISPLAY_POLY
	// draw polygon approximation of b spline
	
	int no_divisions = 128 / NO_CONTROL_POINTS;
	
	if (spline_weights == NULL)
	    spline_weights = new SplineWeights(no_divisions);

	BoundaryPoints *boundary = to_points();
	
	float vert[2];
	
 	setlinestyle(0);
	bgnclosedline();

	for (i = 0; i < boundary->get_no_points(); i++)
	{
	    vert[0] = boundary->point_data()[i].x;
	    vert[1] = boundary->point_data()[i].y;
	    
	    v2f(vert);
	}

	endclosedline();

	delete boundary;
#else
	// draw true b spline to screen using GL
	gl_vec3 geom1[NO_CONTROL_POINTS + 4];
	for (i = 0; i < NO_CONTROL_POINTS + 4; i++)
	{
	    geom1[i][0] = spline[i % NO_CONTROL_POINTS].x + ox;
	    geom1[i][1] = spline[i % NO_CONTROL_POINTS].y + oy;
	    geom1[i][2] = 0.0;
	}
	
	crvn(NO_CONTROL_POINTS+4,geom1);
#endif
	
    }
    if (Profile::draw_fill)
	draw_filled();
    
#ifdef USE_GL
    
    if (Profile::draw_rectangle)
    {
	//get_size();
	sbox((float) (origin.x - width / 2),
	     (float) (origin.y - height/ 2),
	     (float) (origin.x + width / 2),
	     (float) (origin.y + height / 2));
    }
    
#endif
    
    float vert[2];
    vert[0] = spline[0].x + origin.x;
    vert[1] = spline[0].y + origin.y;
    /*bgnpoint();
      v2f(vert);
      endpoint();*/
    if (Profile::draw_boxes)
    {
	for (j = 0; j < NO_CONTROL_POINTS; j++)
	{
// nts: usually (always?) col1==col2==col3 so don't bother setting it all the time
//          if (Profile::draw_in_colour) 
//  	    {
//  		if (j == 0)
//  		    Image::set_colour(col3);
//  		else
//  		    Image::set_colour(col2);
//  	    }
	    vert[0] = spline[j].x + origin.x;
	    vert[1] = spline[j].y + origin.y;
	    rectf((float) (vert[0] - Profile::draw_box_size), 
		  (float) (vert[1] - Profile::draw_box_size),
		  (float) (vert[0] + Profile::draw_box_size), 
		  (float) (vert[1] + Profile::draw_box_size));
	    /* if ((j % 4) == 0)
	       {
	       cmov2(spline[j].x + ox, spline[j].y + oy);
	       char nstr[20];
	       sprintf(nstr, "%d", j);
	       charstr(nstr);
	       }*/
	}