boundarypoints.cc

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

	{
	    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