profile.cc

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

    }
    
    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