nagvector.cc

来自「VC视频对象的跟踪提取原代码（vc视频监控源码）」· CC 代码 · 共 653 行

// 
// NagVector.cc
// 
// A class for manipulating realno n-vectors
//

#include <cassert>
#include <cstdio>
#include <ctime>
#include <sys/types.h>

#include "NagMatrix.h"
#include "tracker_defines_types_and_helpers.h"
#include "EnvParameter.h"
#include "text_output.h"

namespace ReadingPeopleTracker
{

// definition and initialisation of static member variables
EnvParameter NagVector::output_binary_mode("NAG_BINARY_OUTPUT", false);


void NagVector::vector_error(const char *message) const
{
    cerror << "Error in NagVector library routine: "
	   << message << endl;
    abort();
}

NagVector::NagVector(unsigned int n, realno *dt, unsigned int the_alloc_size)
{
    if (the_alloc_size > 0)
	alloc_size = the_alloc_size;
    else
	alloc_size = size = n;

    data = dt;
    own_memory = false;		// we didn't allocate this memory so don't delete it !
}

void NagVector::reconstruct(unsigned int n, unsigned int new_alloc_size)
{
    if ((data != NULL) && (own_memory))
    {
	delete [] data;
	own_memory = false;
    }
    
    alloc_size = size = n;
    
    if (new_alloc_size > 0)            // parameter specified?
	alloc_size = new_alloc_size;
    
    if (alloc_size > 0)
    {
	data = new realno[alloc_size];
	own_memory = true;
    }
    else
    {
	data = NULL;
	own_memory = false;
    }
}

NagVector::NagVector(unsigned int n, unsigned int asize)
{
    alloc_size = size = n;
    
    if (asize > 0)
	alloc_size = asize;
    
    if (alloc_size > 0)
    {
	data = new realno[alloc_size];
	own_memory = true;
    }
    else
    {
	data = NULL;
	own_memory = false;
    }
}

NagVector::NagVector(const NagVector &v)
{
    alloc_size = v.alloc_size;
    size = v.size;
    
    // allocate own memory iff v has its own
    if (v.own_memory)
    {
	data = new realno[alloc_size];
	own_memory = true;

	// copy over all data, not just `size' number of elements
	memcpy((void*)data, (void*)v.data, alloc_size * sizeof(realno));
    }
    else
    {
	data = v.data;
	own_memory = false;
    }
}

NagVector::NagVector(const NagMatrix &m)
{
    alloc_size = size = m.no_rows() * m.no_columns();
    
    // allocate own memory even if m does not have its own    
    data = new realno[alloc_size];
    own_memory = true;
    
    // copy over all data
    memcpy((void*)data, (void*)m.get_data_const(), size * sizeof(realno));
}

NagVector::NagVector(unsigned int n, realno (*func) (unsigned int), unsigned int the_alloc_size)
{
    size = n;
    
    if (the_alloc_size > 0)
	alloc_size = the_alloc_size;
    else
	alloc_size = size;

    data = new realno[alloc_size];
    own_memory = true;

    for (unsigned int i = 0; i < size; i++)
	data[i] = (*func) (i);
}

void NagVector::reset()
{ 
    // this function is obsolete 
    if (own_memory)
	vector_error("bad call to NagVector::reset");
    
    size = alloc_size = 0;
    data = NULL; 
}

NagVector::~NagVector()
{
    if ((data != NULL) && (own_memory))
	delete [] data;
}

// copy n elements from an array of floats
void NagVector::copy_data(float *cdata, unsigned int n)
{
    if (n == 0)
	n = size;

    if ((data == NULL) && (n > 0))
	reconstruct(n);

    if (n > size)
	vector_error("too much data to copy");

    realno *dat1 = data;
    realno *dat2 = &data[n];
    
    while (dat1 < dat2)
	*dat1++ = (realno) *cdata++;
}

// copy n elements from an array of doubles
void NagVector::copy_data(double *cdata, unsigned int n)
{
    if (n == 0)
	n = size;

    if ((data == NULL) && (n > 0))
	reconstruct(n);

    if (n > size)
	vector_error("too much data to copy");

    realno *dat1 = data;
    realno *dat2 = &data[n];
    
    while (dat1 < dat2)
	*dat1++ = (realno) *cdata++;
}


NagVector &NagVector::operator=(const NagVector &vec2)
{
    if ((data != NULL) && (own_memory))
    {
	delete [] data;
	own_memory = false;
    }
    
    alloc_size = vec2.alloc_size;
    size = vec2.size;
    
    // allocate own memory iff vec2 has its own
    if (vec2.own_memory)
    {
	data = new realno[alloc_size];
	own_memory = true;
 
	// copy over all data, not just `size' number of elements
	memcpy((void*)data, (void*)vec2.data, alloc_size * sizeof(realno));
   }
    else
    {
	data = vec2.data;
	own_memory = false;
    }
    
    return *this;
}

void NagVector::output()
{
    for (unsigned int i = 0; i < size; i++)
	cinfo << " [ " << data[i] << " ] " << endl;   // FIXME: cinfo the correct one?
    cinfo << endl;
}

void NagVector::clear(const realno value)
{
    realno *enddata = &data[size];
    while ((--enddata) >= data) *enddata = value;
}

void NagVector::add(const realno item)
{
    if (data == NULL) reconstruct(0,2);
    unsigned int size1 = size + 1;
    if (size1 > alloc_size)
    {
	NagVector new_stack(size1, alloc_size * 2);
	this->copy(new_stack);
	new_stack.data[size] = item;
	*this = new_stack;
    }
    else
    {
	data[size] = item;
	size = size1;
    }
}

void NagVector::add(const NagVector &vec2, NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(MIN(size, vec2.size));
    unsigned int n = MIN (MIN (size, vec2.size), res.size);
    
    realno *vdat1 = data;
    realno *vdat2 = vec2.data;
    realno *rdat = res.data;
    realno *edat = &(data[n]);
    
    while (vdat1 < edat)
	*rdat++ = (*vdat1++) + (*vdat2++);
}

NagVector &NagVector::operator+=(const NagVector &v2)
{
    unsigned int n = MIN(size, v2.size);
    realno *dat1 = data;
    realno *dat2 = v2.data;
    realno *edat = &(data[n]);
    while (dat1 < edat) 
	*dat1++ += *dat2++;
    return *this;
}

NagVector &NagVector::operator-=(const NagVector &v2)
{
    unsigned int n = MIN(size, v2.size);
    realno *dat1 = data;
    realno *dat2 = v2.data;
    realno *edat = &(data[n]);
    while (dat1 < edat) 
	*dat1++ -= *dat2++;
    return *this;
}

NagVector &NagVector::operator*=(const realno a)
{
    realno *dat1 = data;
    realno *edat = &(data[size]);
    while (dat1 < edat) *dat1++ *= a;
    return *this;
}

NagVector &NagVector::operator/=(const realno a)
{
    realno *dat1 = data;
    realno *edat = &(data[size]);
    while (dat1 < edat) *dat1++ /= a;
    return *this;
}

void NagVector::subtract(const NagVector &vec2, NagVector &res) const
{
    if (res.get_data() == NULL)
	res.reconstruct(MIN(size, vec2.get_size()));
 
    assert (res.get_size() >= MAX(size, vec2.get_size()));

    unsigned int n = MIN (MIN (size, vec2.get_size()), res.get_size());

    realno *vdat1 = data;
    const realno *vdat2 = vec2.get_data_const();
    realno *rdat = res.get_data();
    
    unsigned int row;
 
    for (row = 0; row < n; row++)
	*rdat++ = (*vdat1++) - (*vdat2++);
}

void NagVector::copy(NagVector &res) const
{
    if (res.get_data() == NULL)
	res.reconstruct(size);

    assert(size == res.size);
    
//     realno *dat1 = data;
//     realno *rdat = res.data;
//     realno *edat = &(data[MIN(size,res.size)]);
//     while (dat1 < edat) *rdat++ = (*dat1++);
    
    memcpy(res.data, data, MIN(size,res.size) * sizeof(realno));
}


realno NagVector::length2() const
{
    realno *vdat = data;
    realno *edat = &(data[size]);
    realno lngth = 0;
    register realno tmp;

    while (vdat < edat)
    {
	tmp = *vdat++;
	lngth += (tmp * tmp);
    }
    return lngth;
}

void NagVector::scale(const realno lmda, NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(size);
    realno *dat = data;
    realno *edat = &(data[MIN(size,res.size)]);
    realno *rdat = res.data;
    while (dat < edat)
	*rdat++ = lmda * (*dat++);
}

void NagVector::sub_vector(const unsigned int i, const unsigned int j,
			   NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(j-i+1);
    if ((res.size < (j - i + 1)) || (i < 0) || (j >=  size) || (j < i))
	vector_error(" sub_vector  bad parameters ");
    realno *dat = &(data[i]);
    realno *edat = &(data[j]);
    realno *rdat = res.data;
    while (dat <= edat) *rdat++ = *dat++;
}

realno NagVector::dot(const NagVector &vec2) const
{
    realno *dat1 = data;
    realno *dat2 = vec2.data;
    realno *edat = &(data[MIN(size,vec2.size)]);
    realno res = 0;
    while (dat1 < edat)
	res += (*dat1++ * *dat2++);
    return res;
}

void NagVector::convolve(const NagVector &kern, NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(size);
    unsigned int h = (kern.size -1) / 2;
    
    for (unsigned int i = 0; i < size; i++)
    {
	realno itmp = 0.0;
	unsigned int k = 0;
	for (unsigned int j = size-h; j <= size+h; j++)
	    itmp += data[(i+j) % size] * kern[k++];
	res.set (i,itmp);
    }
}


void NagVector::concat(const NagVector &vec2, NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(size + vec2.size);
    if (res.size < (size + vec2.size))
	vector_error("Not enough storage space in NagVector::concat");

    realno *dat1 = data;
    realno *rdat = res.data;
    const realno *edat = &(data[size]);
    
    while (dat1 < edat)
	*rdat++ = (*dat1++);  
    
    dat1 = vec2.data;
    edat = &(vec2[vec2.size]);
    
    while (dat1 < edat)
	*rdat++ = (*dat1++);
}  

void  NagVector::flip(NagVector &res) const
{
    if (res.data == NULL) res.reconstruct(size);
    if (res.size > size)
	vector_error("error in NagVector::flip");
    
    realno *fdat = &(data[size]);
    realno *dat1 = res.data;
    realno *dat2 = &(res.data[res.size]);
    
    while (dat1 < dat2)
	*dat1++ = *(--fdat);
}


ostream &operator<<(ostream &out_s, const NagVector &v)
{
    if ((v.size == 0) || (v.data == NULL))
	return out_s << "0" << endl;
    
    if (NagVector::output_binary_mode.is_on())
    {
#ifdef USE_FLOAT
	out_s << v.size << " f[" << endl;
#else
	out_s << v.size << " d[" << endl;
#endif
	out_s.write((char*) v.data, v.size * sizeof(realno));
	out_s << "]" <<endl;
    }
    else
    {
	out_s << v.size << " [" << endl;
	realno *dat1 = v.data;
	const realno *dat2 = &(v[v.size]);
	while (dat1 < dat2) out_s << (*dat1++) << endl;
	out_s << "]" << endl;
    }
    return out_s;
}

istream &operator>>(istream &in_s, NagVector &v)
{
    char dummy[50];
    unsigned int sz;
    in_s >> sz;
    
    if (v.data == NULL)
	v.reconstruct(sz);
    
    if (v.size < sz)
	v.vector_error("not enough storage space");
    
    if (sz == 0)
	return in_s;
    
    in_s >> dummy;
    if (dummy[0] == 'f')
    {
	in_s.ignore(1);
	if (sizeof(realno) == sizeof(float))
	    in_s.read((char*) v.data, sz * sizeof(float));
	else
	{
	    float *tmp_data = new float[sz];
	    in_s.read((char*) tmp_data, sz * sizeof(float));
	    v.copy_data(tmp_data, sz);
	    delete [] tmp_data;
	}
    }
    else if (dummy[0] == 'd')
    {
	in_s.ignore(1);
	if (sizeof(realno) == sizeof(double))
	    in_s.read((char*) v.data, sz * sizeof(double));
	else
	{
	    double *tmp_data = new double[sz];
	    in_s.read((char*) tmp_data, sz * sizeof(double));
	    v.copy_data(tmp_data, sz);
	    delete [] tmp_data;
	}
    }
    else 
    {
	realno *dat1 = v.data;
	const realno *dat2 = &(v[v.size]);
	while (dat1 < dat2) in_s >> (*dat1++);
    }
    in_s >> dummy;
    return in_s;
}

realno NagVector::sum() const
{
    realno res = 0;

    for (unsigned int i = 0; i < size; i++)
	res += data[i];
    
    return res;
}

#ifdef _SVID_SOURCE
// the following uses SVID 48-bit random numbers...

// generate a normally distributed random variable
unsigned int NagVector::iset = 0;
bool NagVector::seed_set = false;
realno NagVector::gset;

void NagVector::check_seed()
{
    if (seed_set == 0)
    {
	long rseed = time(NULL) + getpid();
	srand48(rseed);
	seed_set = 1;
    }
}

realno NagVector::normal_random()
{
    
    check_seed();
    realno fac,rsq,v1,v2;
    
    if (iset == 0)
    {
	do
	{
	    v1 = 2.0 * drand48() - 1.0;
	    v2 = 2.0 * drand48() - 1.0;
	    rsq = (v1 * v1) + (v2 * v2);
	} while ((rsq >= 1.0 )|| (rsq == 0.0));
	
	fac = sqrt(-2.0 * log(rsq)/rsq);
	gset = v1 * fac;
	iset = 1;
	return v2 * fac;
    }
    
    iset = 0;
    return gset;
    
}

void NagVector::set_unif_random(realno min, realno max)
{
    check_seed();
    realno diff = max - min;
    for (unsigned int i = 0; i < size; i++)
	data[i] = min + (diff * drand48());
}  


void NagVector::set_gauss_random(realno mean, realno sd)
{
    for (unsigned int i = 0; i < size; i++)
	data[i] = mean + (sd * normal_random());
}
#endif // ifdef _SVID_SOURCE


void NagVector::mult_trans(const NagVector &vec2, NagMatrix &res) const
{
    if (res.get_data() == NULL) res.reconstruct(size, vec2.size);

    if ((res.no_rows() != size) || (res.no_columns() != vec2.size))
	vector_error("bad call to mult_trans");
    
    unsigned int i,j;
    for (i = 0; i < size; i++)
	for (j = 0; j < vec2.size; j++)
	    res.set(i, j, data[i] * vec2[j]);
}

realno NagVector::nr_select(unsigned long k, unsigned long n, realno *arr) const
{
    unsigned long i,ir,j,l,mid;
    realno a;
    
    l = 1;
    ir = n;
    for (;;) {    
	if (ir <= l+1) {
	    if (ir == l+1 && arr[ir] < arr[l]) {
		SWAP(arr[l], arr[ir]);
	    }
	    return arr[k];
	} else {
	    mid = (l+ir) >> 1;
	    SWAP(arr[mid],arr[l+1]);
	    if (arr[l] > arr[ir]) {
		SWAP(arr[l],arr[ir]);
	    }
	    if (arr[l+1] > arr[ir]) {
		SWAP(arr[l+1],arr[ir]);
	    }
	    if (arr[l] > arr[l+1]) {
		SWAP(arr[l],arr[l+1]);
	    }
	    
	    i=l+1;
	    j=ir;
	    a=arr[l+1];
	    for (;;) {
		do i++; while (arr[i] < a);
		do j--; while (arr[j] > a);
		if (j < i) break;
		SWAP(arr[i],arr[j]);
	    }
	    arr[l+1]=arr[j];
	    arr[j]=a;
	    if (j >= k) ir=j-1;
	    if (j <= k) l=i;
	}
    }
    return arr[k];
}


realno NagVector::select(realno x) const
{
    NagVector tmp_v;
    copy(tmp_v);
    unsigned long k = (unsigned long) (0.5 + size * x);

    return nr_select(k, size, tmp_v.get_data()-1);
}

} // namespace ReadingPeopleTracker