boundarypoints.cc

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

/*
 * BoundaryPoints.cc
 *
 * the ordered set of points on the boundary of an object
 * 
 */

#include "BoundaryPoints.h"
#include "Profile.h"
#include "SplineMatrix.h"
#include "text_output.h"
#include "SplineWeights.h"

#ifndef NO_DISPLAY
#ifdef USE_GL
#include <gl.h>
#else
#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <vogl.h>
#endif
#endif   // #ifndef NO_DISPLAY

namespace ReadingPeopleTracker
{

// definition and initialisation of static member variables
const realno BoundaryPoints::LARGE =  1e6; // a large number

#ifndef NO_DISPLAY
#ifndef DRAW_POLY
extern NagMatrix bsplinematrix;
#endif   // ifndef DRAW_POLY
#endif   // ifndef NO_DISPLAY


void BoundaryPoints::gauss_smooth(realno sd, unsigned int window_size)
{
    realno *coeff = new realno[2 * window_size + 1];
    realno csum = 0;
    realno denom = 2 * sd * sd;
    unsigned int n,j;
    unsigned int win2 = window_size << 1;
    for (n = -window_size; n <= window_size; n++)
	csum += (coeff[n + window_size] = exp(-((realno) n * (realno) n)/denom));
    for (n = 0; n <= win2; n++)
	coeff[n] /= csum;
    
    PointVector result(no_points);
    Point2 temp;
    for (unsigned int i = 0; i < no_points; i++)
    {
	temp = Point2(0,0);
	j = (i - window_size) + no_points;
	for (n = 0; n <= win2; n++)
	    temp = temp + (coeff[n] * point_data()[j++ % no_points]);
	
	result.point_data()[i] = temp;
    }
    delete [] coeff;
    this->PointVector::operator=(result);
}

void BoundaryPoints::find_best_line(realno &inv_grad, realno &ox, realno &oy)
{
    realno sy2, sx2, sx, sy, sxy, tx, ty;
    sx = sy = sxy = sy2 = sx2 = 0.0;
    Point2 *enddata = end_data();
    for (Point2 *curr = point_data(); curr < enddata; curr++)
    {
	tx = curr->x;
	ty = curr->y;
	sx += tx;
	sy += ty;
	sx2 += (tx * tx);
	sy2 += (ty * ty);
	sxy += (tx * ty);
    }
    
    sx2 -= (sx * sx) / ((realno) no_points);
    sy2 -= (sy * sy) / ((realno) no_points);
    sxy -= (sx * sy) / ((realno) no_points);
    
    realno k = 2 * sxy / (sx2 - sy2);
    realno im1 = k / (-1 - sqrt(1 + k*k));
    realno im2 =  k  / (sqrt(1 + k*k) -1);
    realno err1 = (im1*im1*sy2 - im1*2*sxy + sx2) / (im1 * im1 + 1);
    realno err2 = (im2*im2*sy2 - im2*2*sxy + sx2) / (im2 * im2 + 1);
    if (err1 <= err2) inv_grad = im1;
    else inv_grad = im2;
    
    ox = sx / no_points;
    oy = sy / no_points;
    
}

void BoundaryPoints::find_robust_best_line(realno &inv_grad, realno &ox, realno &oy)
{
    Point2 *curr;
    // first robust center
    find_best_line(inv_grad, ox, oy);
    BoundaryPoints trimmed_pnts(no_points);
    copy(trimmed_pnts);
    for (unsigned int lp = 0; lp < 40; lp++)
    {
	realno sum_dist = 0;
	realno sum_sq_dist = 0;
//	Point2 n(inv_grad, 1);
	unsigned int np = trimmed_pnts.get_no_points();
	cdebug << " " << lp << " " << np << " (" << ox << ", " << oy << ") " << endl;
	
	for (curr = trimmed_pnts.point_data(); 
	     curr < trimmed_pnts.end_data(); curr++)     
	{
	    realno d = fabs((*curr - Point2(ox,oy)).length());
	    sum_dist += d;
	    sum_sq_dist += d * d;
	}
	realno mean_dist = sum_dist / np;
	realno sd_dist = 
	    sqrt((sum_sq_dist / np) - (mean_dist * mean_dist));
	
	realno thresh_d = mean_dist + (1.6 * sd_dist);
	BoundaryPoints new_trimmed;
	for (curr = trimmed_pnts.point_data(); 
	     curr < trimmed_pnts.end_data(); curr++)     
	{
	    realno d = fabs((*curr - Point2(ox,oy)).length());
	    if (d < thresh_d) new_trimmed.add_point(*curr);
	}
	trimmed_pnts = new_trimmed;
	if (trimmed_pnts.no_points < 4) break;
	trimmed_pnts.find_best_line(inv_grad, ox, oy);
    }
    inv_grad = 0;
}


void BoundaryPoints::find_end_highest(Point2 &n, Point2 &origin)
{
    unsigned int highest = 0;
    realno height = -1e6; 
    unsigned int i;

    for (i = 0; i < no_points; i++)
	if (point_data()[i].y > height)
	{
	    highest = i;
	    height = point_data()[i].y;
	}
    
    PointVector result(no_points);
    for (i = 0; i < no_points; i++)
	result.point_data()[i] = point_data()[(highest++ % no_points)] - origin;
    this->PointVector::operator=(result);
    anchor_point = highest; 
}



void BoundaryPoints::find_end(Point2 &n, Point2 &origin)
{
    unsigned int closest = 0, closest2 = 0;
    realno best_dist,  dist;
    realno best_dist2;
    best_dist = 1e6;
    best_dist2 = 1e6;
    unsigned int i;
    
    for (i = 0; i < no_points; i++)
    {
	Point2 p =  point_data()[i] - origin;
	dist = fabs(p^n);
	realno side = p*n;
	
	if (side >= 0)
	{
	    if (dist < best_dist)
	    {
		closest = i;
		best_dist = dist;
	    }
	}
	else
	{
	    if (dist < best_dist2)
	    {
		closest2 = i;
		best_dist2 = dist;
	    }
	}
    }
    PointVector result(no_points);
    closest2 = (closest2 - closest + no_points) % no_points;
    for (i = 0; i < no_points; i++)
	result.point_data()[i] = point_data()[(closest++ % no_points)] - origin;
    this->PointVector::operator=(result);
    anchor_point = closest2;
}

void BoundaryPoints::find_ends(realno inv_grad, realno ox, realno oy)
{
    unsigned int closest = 0, closest2 = 0;
    realno best_dist,  dist;
    realno best_dist2;
    best_dist = 1e6;
    best_dist2 = 1e6;
    unsigned int i;
    
    for (i = 0; i < no_points; i++)
    {
	dist = fabs((point_data()[i].x - ox) - 
		    inv_grad * (point_data()[i].y - oy));
	if (point_data()[i].y > oy)
	{
	    dist = (20 * dist - point_data()[i].y);
	    if (dist < best_dist)
	    {
		closest = i;
		best_dist = dist;
	    }
	}
	else
	{
	    dist = (20 * dist + point_data()[i].y);
	    if (dist < best_dist2)
	    {
		closest2 = i;
		best_dist2 = dist;
	    }
	}
    }
    PointVector result(no_points);
    Point2 centre(ox,oy);
    closest2 = (closest2 - closest + no_points) % no_points;
    for (i = 0; i < no_points; i++)
	result.point_data()[i] = point_data()[(closest++ % no_points)] - centre;
    this->PointVector::operator=(result);
    anchor_point = closest2;
}

void BoundaryPoints::recenter(realno ox, realno oy)
{
    Point2 centre(ox,oy);
    Point2 *lastpnt = end_data();
    while ((--lastpnt) >= point_data()) *lastpnt = *lastpnt - centre;
}

void BoundaryPoints::draw_points(int xlo, int ylo)
{
#ifndef NO_DISPLAY
    float vert[2];
    //color(RED);
    bgnpoint();
    Point2 *enddata = end_data();
    for (Point2 *curr = point_data(); curr < enddata; curr++)
    {
	vert[0] = curr->x + xlo;
	vert[1] = curr->y + ylo;
	v2f(vert);
    }
    endpoint();
    /*color(GREEN);
      bgnline();
      while (curr != NULL)
      {
      vert[0] = curr->dat->x + xlo;
      vert[1] = curr->dat->y + ylo;
      v2f(vert);
      curr = curr->next;
      }
      endline();*/
#endif   // #ifndef NO_DISPLAY
}

// locally optimise the parameter values associated
// with each data point to obtain the best fit
// -- this method is not guaranteed to work in general !
bool BoundaryPoints::find_optimal_spline(Profile *result)
{
    // setup initial parameter values
    filter();
    bool have_filtered = true;
    NagVector u_values(no_points);
    u_values[0] = 0.0;
    realno u_n = no_points;
    unsigned int i;
    if (!have_filtered)
	for (i = 1; i < no_points; i++)
	{
	    u_values[i] = u_values[i-1]
		+ (point_data()[i] - point_data()[i-1]).length();
	    u_n = u_values[no_points-1]
		+ (point_data()[0] - point_data()[no_points-1]).length();
	}
    else
	for (i = 1; i < no_points; i++)
	    u_values[i] = i;
    
    u_values.scale(Profile::NO_CONTROL_POINTS / u_n, u_values);
    
    // find  corresponding spline
    
    realno rms_change = 1e6;
    realno old_rms_change;
    realno curve_err = 1e6;
    realno old_curve_err;
    NagVector del_u(no_points);
    NagVector del_v(no_points);
    NagVector old_u(no_points);
    do 
    {
	u_values.copy(old_u);
	realno u_change = 0.0;
	old_curve_err = curve_err;
	curve_err = 0;
	calculate_spline(u_values, this, result);
//      result->draw(true);
	for (i = 0; i < no_points; i++)