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

/*
 * 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 
#else
#include 
#include 
#include 
#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++)
	{
	    Point2 p, dp, ddp;
	    p.x = p.y = dp.x = dp.y = ddp.x = ddp.y = 0.0;
	    for (unsigned int j = 0; j < Profile::NO_CONTROL_POINTS; j++)
	    {
		p +=(SplineWeights::B_func(j, u_values[i]) * result->point_data()[j]);
		dp += (SplineWeights::dB_func(j, u_values[i]) *
		       result->point_data()[j]);
		ddp += (SplineWeights::ddB_func(j, u_values[i]) *
			result->point_data()[j]);
	    }
	    
// check max error too ?
	    
	    Point2 err_u = point_data()[i] - p;
	    curve_err += err_u.length2();
	    
	    realno du_max ;
	    realno du_min ;
	    du_max = 1.0 / Profile::NO_CONTROL_POINTS;
	    du_min = - du_max;
	    
	    del_u[i]  = - 1.0 * (err_u * dp) / ((err_u * ddp)  - (dp * dp));
	    
	    if (del_u[i] >= du_max)
		del_u[i] = du_max;
	    if (del_u[i] <= du_min)
		del_u[i] =  du_min;
	    
	    //if (du_max < du_min) abort();
	    
	}
	//u_change = del_u.length2();
	u_values.add(del_u, u_values);
	// make sure u_0 = 0
	for (i = 0 ; i < no_points; i++)
	    u_values[i] -= (u_values[0]);
	
	for (i = 1; i < no_points; i++)
	{
	    del_v[i-1] = u_values[i] - u_values[i-1];
	    if (del_v[i-1] < 0)
		del_v[i-1] = 0;
	}
	
	del_v[no_points-1] = u_values[0] + Profile::NO_CONTROL_POINTS -
	    u_values[no_points-1];
	
	del_v.scale(Profile::NO_CONTROL_POINTS / del_v.sum(), del_v);
	
	for (i = 1; i < no_points; i++)
	    u_values[i] = u_values[i-1] + del_v[i-1];
	
	old_u.subtract(u_values,del_u);
	
	old_rms_change = rms_change;
	u_change = del_u.length2();
	rms_change = sqrt(u_change / no_points);
//      cdebug << " rms_change " << rms_change << " curve_err " << curve_err << endl;
//      result->draw(false);
	
    } while ((fabs(rms_change - old_rms_change) > 1e-3) &&
	     (curve_err < old_curve_err));
    
    if (old_curve_err < curve_err) 
	calculate_spline(old_u, this, result);
    
    return true;
}

bool BoundaryPoints::filter()
// select data points by arc-length division 
{
    unsigned int SUB_N = 32;		
    unsigned int indx = 0;
    
    // choose a precomputed points-to-spline matrix
    // so that
    // # boundary points > # new sampled boundary points
    // but only just !
    // 
    // Hence start with RHS large and decrease until
    // condition satisfied 
    
    while ((no_points < (Profile::NO_CONTROL_POINTS * SUB_N)) && (SUB_N >= 2))
    {
	SUB_N /= 2;
	indx++;
    }
    
    // if the setup function has not been called yet, initialise
    // the required matrix
    if (s_matrices[indx] == NULL)
	s_matrices[indx] = new SplineMatrix(SUB_N);
    
    
    // pass1 - store all lengths 
    realno *arc = new realno[no_points+1];
    arc[0] = 0;
    Point2 diff;
    Point2 *input_pnts = point_data();
    unsigned int i;
    for (i = 0; i < no_points; i++)
    {
	diff = input_pnts[(i+1) % no_points] - input_pnts[i];
	arc[i+1] = arc[i] + diff.length();
    }
    
    realno total_arclength = arc[no_points];
    
    // pass2 - select relevant points
    
    // set the spacing interval for parametric values
    realno interval = arc[no_points] / ((realno)(Profile::NO_CONTROL_POINTS * SUB_N));
    
    unsigned int curr_pos = 1;
    unsigned int new_no_points = Profile::NO_CONTROL_POINTS * SUB_N;
    
    BoundaryPoints result(new_no_points);
    result.point_data()[0] = point_data()[0];
    realno l1,l2, iarc;
    for (i = 1; i < new_no_points; i++)
    {
	iarc = ((realno)i) * interval;
	
	// skip points if necessary
	while (arc[curr_pos] < iarc)
	    curr_pos++;
	
	if (curr_pos > no_points)
	{
	    cerror << " Error in BoundaryPoints::filter(): no matching points "
		   << endl;
	    
	    exit(1);
	}
	
	// use linear interpolation of lengths to define new point
	l1 = arc[curr_pos-1]; 
	l2 = arc[(curr_pos) % no_points];
	// check for gone full circle
	if (curr_pos == no_points) l2 += total_arclength;
	
	realno lmda = (iarc - l1) / (l2 - l1);
	
	result.point_data()[i] = point_data()[curr_pos-1] * (1.0 - lmda) 
	    + point_data()[(curr_pos) % no_points] * lmda;
	
    }
    delete [] arc;
    *this = result;
    return true;
}


bool BoundaryPoints::convert_to_spline(PointVector *result)
{
    if (filter() == false)
	return false;

    to_spline(result);

    return true;
}

void BoundaryPoints::to_spline(PointVector *result)
{
    if (result->get_no_points() < Profile::NO_CONTROL_POINTS)
    {
	cerror << " Error in BoundaryPoints::to_spline(): too many control points requested \n"
	       << endl;
	
	exit(1);
    }
    
    // get the relevant points-to-spline matrix 
    // (again !!)
    unsigned int SUB_N = no_points / Profile::NO_CONTROL_POINTS;
    unsigned int temp_n = SUB_N;
    unsigned int indx = 0;
    
    while (temp_n < 32)
	temp_n *= 2; indx++;
    
    if (s_matrices[indx] == NULL)
	s_matrices[indx] = new SplineMatrix(SUB_N);
    
    SplineMatrix *s_matrix = s_matrices[indx];
    
    // calculate the RHS of the simultaneous equations
    realno *vdat = s_matrix->vdata;
    unsigned int i; unsigned int j; unsigned int l; unsigned int i2; 
    NagVector rhsx(Profile::NO_CONTROL_POINTS);
    NagVector rhsy(Profile::NO_CONTROL_POINTS);
    unsigned int ntotal = SUB_N * Profile::NO_CONTROL_POINTS;
    realno temp; 
    // get the corresponding vectors 
    for (l = 0; l < Profile::NO_CONTROL_POINTS; l++)
    {
	rhsx.get_data()[l] = 0;
	rhsy.get_data()[l] = 0;
	j = 0;
	for (i = (l - 2) * SUB_N;i <= (l + 2) * SUB_N; i++)
	{
	    i2 = (i + ntotal) % ntotal;
	    temp = vdat[j++];
	    rhsx.get_data()[l] += temp * (point_data()[i2].x);
	    rhsy.get_data()[l] += temp * (point_data()[i2].y);
	}
    }
    NagVector resx(Profile::NO_CONTROL_POINTS);
    NagVector resy(Profile::NO_CONTROL_POINTS);
    s_matrix->multiply(rhsx,resx);
    s_matrix->multiply(rhsy,resy);
    Point2 *spln = result->point_data();
    for (l = 0; l < Profile::NO_CONTROL_POINTS; l++)
	*spln++ = Point2(resx[l],resy[l]);
}


void BoundaryPoints::draw_curve(int ox, int oy, int step)
{
#ifndef NO_DISPLAY
#ifndef DISPLAY_POLY
    defbasis(1,bsplinematrix);
    curveprecision(15);
    linewidth(2);
    Coord geom1[4][3];
    unsigned int i,j;
    geom1[0][2] = geom1[1][2] = geom1[2][2] = geom1[3][2] = 0;
    //char test_str[50];
    for (i = 0; i < no_points; i += step)
    {
	geom1[0][0] = point_data()[i].x + ox;
	geom1[0][1] = point_data()[i].y + oy;
	j = (i+step) % no_points;
	geom1[1][0] = point_data()[j].x + ox;
	geom1[1][1] = point_data()[j].y + oy;
	j = (j+step) % no_points;
	geom1[2][0] = point_data()[j].x + ox;
	geom1[2][1] = point_data()[j].y + oy;
	j = (j+step) % no_points;
	geom1[3][0] = point_data()[j].x + ox;
	geom1[3][1] = point_data()[j].y + oy;
	crv(geom1);
    }
#endif   // ifndef DISPLAY_POLY
#endif   // ifndef NO_DISPLAY
}


//////////////////////////////////////////////////////////////////////////
//									//
//  alternative methods ....						//
//									//
//////////////////////////////////////////////////////////////////////////


// calculate a spline from an arbitrary set of Boundary points
// without worrying about speed / efficiency
void BoundaryPoints::calculate_spline(NagVector &u_values, PointVector *data,
				      PointVector *result)
{
    NagMatrix M(Profile::NO_CONTROL_POINTS, Profile::NO_CONTROL_POINTS);
    M.clear(0);
    unsigned int n = u_values.get_size();
    unsigned int i;
    unsigned int j;
    unsigned int k;
    
    for (j = 0; j < Profile::NO_CONTROL_POINTS; j++)
	for (k = 0; k < Profile::NO_CONTROL_POINTS; k++)
	    for (i = 0; i < n; i++)
		*M.get(j,k) += SplineWeights::B_func(j, u_values[i]) *
		    SplineWeights::B_func(k,u_values[i]);
    
    
    NagMatrix B(Profile::NO_CONTROL_POINTS, 2);
    NagVector rhs_x(Profile::NO_CONTROL_POINTS, B.get(0,0));
    NagVector rhs_y(Profile::NO_CONTROL_POINTS, B.get(0,1));
    
    rhs_x.clear(0);
    rhs_y.clear(0);
    
    
    for (j = 0; j < Profile::NO_CONTROL_POINTS; j++)
	for (i = 0; i < n; i++)
	{
	    realno tmp = SplineWeights::B_func(j, u_values[i]);
	    rhs_x[j] += tmp * data->point_data()[i].x;
	    rhs_y[j] += tmp * data->point_data()[i].y;
	}
    rhs_x.reset();
    rhs_y.reset();
    
    NagMatrix C;
    M.solve_equations(B, C);
    
    for (j = 0; j < Profile::NO_CONTROL_POINTS; j++)
	result->point_data()[j] = Point2(C.read(j,0), C.read(j, 1));
}


void BoundaryPoints::setup_default_spline_matrices()
{
    cdebug << " BoundaryPoints: setting up default spline matrices ..." << endl;

    s_matrices[0] = new SplineMatrix(32);
    s_matrices[1] = new SplineMatrix(16);
    s_matrices[2] = new SplineMatrix(8);
    s_matrices[3] = new SplineMatrix(4);
    s_matrices[4] = new SplineMatrix(2);
    s_matrices[5] = new SplineMatrix(1);
    
}

} // namespace ReadingPeopleTracker