www.pudn.com > ReadingPeopleTracker-1.28.rar > Profile.cc

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

#include   // for hypot
#include 

#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 

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

#else  // ifdef USE_GL

#include 
#include 
#include 

#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 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);
	       }*/
	}
    }
    
    if (Profile::draw_arrow)
    {
//  	if (Profile::draw_in_colour)
//  	    Image::set_colour(col3);
	if (height == 0)
	    get_size();
	Point2 arr_start(origin.x, origin.y + 0.2 * height);
	double arr0 = 0.2 * height;
	double arr1 = 5 * arr0;
	double arr2 = 4 * arr0;
//	linewidth(Profile::draw_linewidth);
	bgnline();
	vert[0] = arr_start.x;
	vert[1] = arr_start.y;
	v2f(vert);
	vert[0] = arr_start.x + arr1 * direction.x;
	vert[1] = arr_start.y + arr1 * direction.y;
	v2f(vert);
	vert[0] = arr_start.x + arr2 * direction.x - arr0 * direction.y;
	vert[1] = arr_start.y + arr2 * direction.y + arr0 * direction.x;
	v2f(vert);
	vert[0] = arr_start.x + arr1 * direction.x;
	vert[1] = arr_start.y + arr1 * direction.y;
	v2f(vert);
	vert[0] = arr_start.x + arr2 * direction.x + arr0 * direction.y;
	vert[1] = arr_start.y + arr2 * direction.y - arr0 * direction.x;
	v2f(vert);  
	endline();
//	linewidth(Profile::draw_linewidth);
    }
    setlinestyle(0);
    
//  nts: put into Profile
//      if (Profile::draw_label)
//      {
//  	cmov2(ox - width  / 2, oy - height / 2 - 12);
//  	char ref_name[32];
//  	sprintf(ref_name,"person %i",ref_no);
//  	charstr(ref_name);   // still in the same colour as the rest  :-)
//      }
#endif   // #ifndef NO_DISPLAY
}

void Profile::scale(realno scale_fac)
{
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
	spline[i] = scale_fac * spline[i];
}

void Profile::scale_coords(realno scale_fac)
{
    scale(scale_fac);
    origin *= scale_fac;
    width *= scale_fac;
    height *= scale_fac;
    direction *= scale_fac;
}

//  realno Profile::get_size()
//  {
//      realno ymin = LARGE;
//      realno ymax = -LARGE;
//      realno xmin = LARGE;
//      realno xmax = - LARGE;
//      for (int  i = 0; i < NO_CONTROL_POINTS; i++)
//      {
//  	if (spline[i].y < ymin) ymin = spline[i].y;
//  	if (spline[i].y > ymax) ymax = spline[i].y;
//  	if (spline[i].x < xmin) xmin = spline[i].x;
//  	if (spline[i].x > xmax) xmax = spline[i].x;
//      }
//      width = (xmax - xmin);
//      height = (ymax - ymin);
//      return 0.5 * (height + width);
//  }

//  void Profile::get_y_bounds(realno &ymin, realno &ymax)
//  {
//      ymin = LARGE;
//      ymax = -LARGE;
//      for (int  i = 0; i < NO_CONTROL_POINTS; i++)
//      {
//  	if (spline[i].y < ymin) ymin = spline[i].y;
//  	if (spline[i].y > ymax) ymax = spline[i].y;
//      }
//  }

void Profile::update_size_variables()
{
    // first, calculate bounding box (variables Observation::xlo etc)
    realno ymin = LARGE;
    realno ymax = -LARGE;
    realno xmin = LARGE;
    realno xmax = - LARGE;
    
    for (int  i = 0; i < NO_CONTROL_POINTS; i++)
    {
	if (spline[i].y < ymin)
	    ymin = spline[i].y;
	if (spline[i].y > ymax)
	    ymax = spline[i].y;
	if (spline[i].x < xmin)
	    xmin = spline[i].x;
	if (spline[i].x > xmax)
	    xmax = spline[i].x;
    }
    xlo = (int) ((xmin - 0.5) + origin.x);
    xhi = (int) ((xmax + 0.5) + origin.x);
    ylo = (int) ((ymin - 0.5) + origin.y);
    yhi = (int) ((ymax + 0.5) + origin.y);    
    
    // now adjust size variables
    width = xhi - xlo + 1;
    height = yhi - ylo + 1;
}

void Profile::normalise(bool flag)
{
    recenter();
    if (!flag)
	scale (get_size() / 200.0);
    // recenters points, rotates and scales
    realno sqsin = 1 / (1 + inv_grad * inv_grad);
    realno sqcos = (1 - sqsin);
    realno sinang;
    realno cosang;
    sinang = sqrt(sqsin);
    if (inv_grad >= 0)
	cosang = sqrt(sqcos);
    else cosang = - sqrt(sqcos);
    if (!flag)
	cosang = - cosang;
    Point2 newx(sinang,-cosang);
    Point2 newy(cosang,sinang);
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
	spline[i] = Point2(newx * spline[i], newy * spline[i]);
    get_size();
    if (flag)
	scale(200.0/get_size());
}

void Profile::recenter(realno *w)
{
    Point2 sum(0,0);
//    realno wt = (1.0 / NO_CONTROL_POINTS);
    if (w != NULL)
    {
	realno wsum = 0;
	for (int i = 0;i < NO_CONTROL_POINTS; i++)
	{
	    sum += w[i] * spline[i];
	    wsum += w[i];
	}
	sum = (1.0 / wsum) * sum;
    }
    else
    {
	realno ymin = LARGE;
	realno ymax = -LARGE;
	realno xmin = LARGE;
	realno xmax = - LARGE;
	for (int  i = 0; i < NO_CONTROL_POINTS; i++)
	{
	    if (spline[i].y < ymin) ymin = spline[i].y;
	    if (spline[i].y > ymax) ymax = spline[i].y;
	    if (spline[i].x < xmin) xmin = spline[i].x;
	    if (spline[i].x > xmax) xmax = spline[i].x;
	}
	sum = Point2((xmax + xmin) / 2.0, (ymax + ymin) / 2.0);
    }
    origin += sum;
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
	spline[i] -= sum;
}

void Profile::map(const realno &ax, const realno &ay, Profile *result)
{
    if (result == NULL) result = this;
    PointVector::map(ax,ay,*result);
}

void Profile::align_to(Profile *mean, Profile *result,
		       realno &s, realno &th,
		       realno *weights)
{
    
    //mean->recenter(weights);
    recenter(weights);
    realno C1, C2, Z;
    C1 = C2 = Z = 0;
    Point2 *prf1 = &(mean->spline[0]);
    Point2 *prf2 = &(spline[0]);
    realno tmp = (1.0 / NO_CONTROL_POINTS);
    for (int k = 0; k < NO_CONTROL_POINTS; k++)
    {
	if (weights != NULL) tmp = weights[k];
	Z += tmp * (prf2[k].x * prf2[k].x + prf2[k].y * prf2[k].y);
	C1 += tmp * (prf1[k].x * prf2[k].x + prf1[k].y * prf2[k].y);
	C2 += tmp * (prf1[k].y * prf2[k].x - prf1[k].x * prf2[k].y);
    }
    
    realno ax = C1 / Z;
    realno ay = C2 / Z;
    
    // hypot(X,Y) returns `sqrt(X*X + Y*Y)'.
    s = hypot(ax,ay);
    th = (atan2(ay,ax));
    
    map(ax,ay,result);
}

void Profile::display_shape(realno new_ox, realno new_oy)
{
    Point2 saved_origin = origin;
    origin = Point2(new_ox,new_oy);
    draw();
    origin = saved_origin;
}

Profile *Profile::reflect(Profile *result)
{
    if (result == NULL)
	result = new Profile;
    
    result->origin = origin;
    result->width = width;
    result->height = height;
    result->direction = Point2(-direction.x, direction.y);
    result->spline[0] = Point2(-spline[0].x, spline[0].y);
    
    for (int i = 1; i < NO_CONTROL_POINTS; i++)
	result->spline[NO_CONTROL_POINTS - i]
	    = Point2(- spline[i].x, spline[i].y);
    return result;
}

realno *Profile::get_shape(int i)
{
    // if (i == (2 * NO_CONTROL_POINTS)) return &(direction.x);
    // if (i == (2 * NO_CONTROL_POINTS + 1)) return &(direction.y);
    if ((i % 2) == 0)
	return &(spline[i / 2].x);
    else
	return &(spline[i / 2].y);
}

realno Profile::get_data(int i)
{
    if (i < (2*NO_CONTROL_POINTS))
	return *get_shape(i);
    if (i == (2*NO_CONTROL_POINTS))
	return direction.x;
    if (i == (2 * NO_CONTROL_POINTS + 1))
	return direction.y;
    
    /* NOTREACHED */
    return 0;
}

void Profile::set_data(int i, realno val)
{
    if (i < (2*NO_CONTROL_POINTS))
	*get_shape(i) = val;
    if (i == (2 * NO_CONTROL_POINTS))
	direction.x = val;
    if (i == (2 * NO_CONTROL_POINTS + 1))
	direction.y = val;
}

int Profile::get_data_size()
{
    if (Profile::draw_arrow)
	return 2 * NO_CONTROL_POINTS+2;
    return 2 * NO_CONTROL_POINTS;
}

void Profile::draw_filled()
{
#ifndef NO_DISPLAY
#ifndef DISPLAY_POLY
    if (spline_weights != NULL)
	delete spline_weights;
    int no_divisions = 128 / NO_CONTROL_POINTS;
    spline_weights = new SplineWeights(no_divisions);
    BoundaryPoints *boundary = to_points(NULL);
    float vert[2];
    concave(true);
    polymode(PYM_FILL);
    bgnpolygon();
    for (int i = 0; i < boundary->get_no_points(); i++)
    {
	vert[0] = boundary->point_data()[i].x;
	vert[1] = boundary->point_data()[i].y;
	v2f(vert);
    }
    endpolygon();
    delete boundary;
#endif   // ifndef DISPLAY_POLY
#endif   // ifndef NO_DISPLAY
}

BoundaryPoints *Profile::to_points(BoundaryPoints *res)
{
    setup_spline_weights();
    
    if (res == NULL)
	res = new BoundaryPoints(NO_CONTROL_POINTS * spline_weights->nsubs);
    else
	if (res->get_no_points() < (NO_CONTROL_POINTS * spline_weights->nsubs))
	{
	    cerror << " error in Profile::ro_points " << endl;
	    abort();
	}
    int k;
    Point2 p0,p1,p2,p3;
    Point2 *curr = res->point_data();
    
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
    {
	k = (i-1); 
	if (k < 0)
	    k += NO_CONTROL_POINTS;
	p0 = spline[k];
	p1 = spline[(k+1) % NO_CONTROL_POINTS];
	p2 = spline[(k+2) % NO_CONTROL_POINTS];
	p3 = spline[(k+3) % NO_CONTROL_POINTS];
	for (int j = 0; j < spline_weights->nsubs; j++)
	{
	    *curr++ = origin
		+ (spline_weights->w0[j] * p0) + (spline_weights->w1[j] * p1)
		+ (spline_weights->w2[j] * p2) + (spline_weights->w3[j] * p3);
	}
    }
    return res;
}

void Profile::interpolate(NagVector &coeff, NagVector &res)
{
    setup_spline_weights();
    
    if ((coeff.get_size() != NO_CONTROL_POINTS)
	|| (res.get_size() != (NO_CONTROL_POINTS * spline_weights->nsubs)))
    {
	cerror << " error in Profile::interpolate " << endl;
	abort();
    }
    int k;
    realno p0,p1,p2,p3;
    realno *curr = res.get_data();
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
    {
	k = (i-1); 
	if (k < 0) k += NO_CONTROL_POINTS;
	p0 = coeff[k];
	p1 = coeff[(k+1) % NO_CONTROL_POINTS];
	p2 = coeff[(k+2) % NO_CONTROL_POINTS];
	p3 = coeff[(k+3) % NO_CONTROL_POINTS];
	for (int j = 0; j < spline_weights->nsubs; j++)
	{
	    *curr++ = (spline_weights->w0[j] * p0) + (spline_weights->w1[j] * p1)
		+ (spline_weights->w2[j] * p2) + (spline_weights->w3[j] * p3);
	}
    }
}

void Profile::get_normals(Profile &res)
{ 
    int k;
    Point2 p0, p2, tmp;
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
    {
	k = (i-1); 
	if (k < 0)
	    k += NO_CONTROL_POINTS;
	p0 = spline[k];
	p2 = spline[(k+2) % NO_CONTROL_POINTS];
	tmp = 0.5 * (p2 - p0);
	tmp.normalise();
	res.spline[i] = Point2(-tmp.y, tmp.x);
    }
}

BoundaryPoints *Profile::sample_normals(BoundaryPoints *res)
{ 
    setup_spline_weights();
    
    if (res == NULL)
	res = new BoundaryPoints(NO_CONTROL_POINTS * spline_weights->nsubs);
    else
	if (res->get_no_points() < (NO_CONTROL_POINTS * spline_weights->nsubs))
	{
	    cerror << " error in Profile::sample_normals " << endl;
	    abort();
	}
    
    int k;
    Point2 p0,p1,p2,p3;
    Point2 *curr = res->point_data();
    for (int i = 0; i < NO_CONTROL_POINTS; i++)
    {
	k = (i-1); 
	if (k < 0)
	    k += NO_CONTROL_POINTS;
	p0 = spline[k];
	p1 = spline[(k+1) % NO_CONTROL_POINTS];
	p2 = spline[(k+2) % NO_CONTROL_POINTS];
	p3 = spline[(k+3) % NO_CONTROL_POINTS];
	for (int j = 0; j < spline_weights->nsubs; j++)
	{
	    *curr =
		(spline_weights->dw0[j] * p0) + (spline_weights->dw1[j] * p1) +
		(spline_weights->dw2[j] * p2) + (spline_weights->dw3[j] * p3);
	    curr->normalise();
	    *curr = Point2(-curr->y, curr->x);
	    curr++;
	}
	
    }
    return res;
}

void Profile::points_to_spline()
{
    // convert a set of points to a spline 
    BoundaryPoints *pnts = new BoundaryPoints(NO_CONTROL_POINTS);
    copy(*pnts);
    pnts->to_spline(this);
    delete pnts;
}

void Profile::draw_in_image(Image *canvas, Point2 offset, bool filled)
{
    if (spline_weights == NULL)
	spline_weights = new SplineWeights(8);
    
    Point2 saved_origin = origin;
    origin = offset;  // FIXME: should this be "+=" ?
    
    BoundaryPoints *prf_outline = to_points();
    int np = prf_outline->get_no_points();
    Point2 *pdata = prf_outline->point_data();
    if (filled)
    {
	canvas->draw_filled_polygon(np, pdata);
    }
    for (int i = 0; i < np; i++)
    {
	Point2 p1 = pdata[i];
	Point2 p2 = pdata[(i+1)%np];
	canvas->set_line_width(3.0);
	canvas->draw_line(p1.x+0.5,p1.y+0.5,p2.x+0.5,p2.y+0.5);
    }
    
    // restore original origin
    origin = saved_origin;
    
    delete prf_outline;
}


void Profile::draw()
{
#ifndef NO_DISPLAY
    if (filters_initialised == false)
	return;
    
    if (is_visible == false)
	return;
    
    if (Profile::draw_linewidth > 1)
    {
	int old_draw_linewidth = Profile::draw_linewidth;
	Profile::draw_linewidth = 1;
	Profile::draw(0,0,0);
	Profile::draw_linewidth = old_draw_linewidth;
    }
    else
    {
	Profile::draw(0,0,0);
    }

#endif   // #ifndef NO_DISPLAY
}


void Profile::get_enclosing_box(int &xmin, int &xmax,
				int &ymin, int &ymax)
{
    // NB make sure update_size_variables() was called!
    xmin = xlo;   xmax = xhi;   ymin = ylo;   ymax = xhi;
    
    int dx = (int) (0.5 + sqrt(pos_filter->get_cov().val00));
    int dy = (int) (0.5 + sqrt(pos_filter->get_cov().val11));
    
    int max_dx = (int) (0.5 + (xmax - xmin) / 4);
    int max_dy = (int) (0.5 + (ymax - ymin) / 4);
    
    if (dx > max_dx) 
	dx = max_dx;
    
    if (dy > max_dy) 
	dy = max_dy;
    
    xmin -= dx;
    xmax += dx;
    ymin -= dy;
    ymax += dy;    
}  

void Profile::update_shape_filters()
{
    if ((this == NULL) || (filters == NULL))
    {
	cerror << " Profile::update_shape_filters(): bad call to method " << endl;
	return;
    }
    for (int i = 0; i < no_shape_parameters; i++)
	filters[i]->update_state();    
}


void Profile::setup_spline_weights()
{
    if (spline_weights == NULL)
	spline_weights = new SplineWeights();
}

} // namespace ReadingPeopleTracker