nagmatrix.cc

VC视频对象的跟踪提取原代码（vc视频监控源码）

    int LDA = (int) rows;
    int LDB = (int) m2.rows;
    int LDC = (int) result.rows;
    
    realno *B = m2.data;
    realno *C = result.data;
    realno *A = data;
    dgemm_(&transa, &transb, &M, &N, &K, &alpha, A, &LDA,
	   B, &LDB, &beta, C, &LDC);
}

void NagMatrix::multiply(const NagVector &m, NagVector &r) const
{
    if (r.get_data() == NULL) r.reconstruct(rows);
    if ((columns > m.get_size()) || (rows > r.get_size()))
	matrix_error(" vector too small \n");
    char trans = 'N';
    int M = (int) rows;
    int N = (int) columns;
    realno alpha = 1.0;
    realno beta = 0.0;
    int LDA = (int) rows;
    const realno *X = m.get_data_const();
    int INCX = 1;
    realno *Y = r.get_data();
    realno *A = data;
    int INCY = 1;
    dgemv_(&trans,&M, &N, &alpha, A, &LDA, X, &INCX, &beta, Y, &INCY);
}

void NagMatrix::multiply(const NagVector &m, NagVector &r, char trans,
			 realno alpha)
{
    if (trans == 't') trans = 'T';
    if (trans == 'n') trans = 'N';
    int new_rows = rows;
    int new_cols = columns;
    
    if (trans == 'T') 
    {
	new_rows = columns;
	new_cols = rows;
    }

    if (r.get_data() == NULL) r.reconstruct(new_rows);
    if ((new_cols > m.get_size()) || (new_rows > r.get_size()))
	matrix_error(" vector too small \n");
    
    int M = (int) rows;
    int N = (int) columns;
    realno beta = 0.0;
    int LDA = (int) rows;
    const realno *X = m.get_data_const();
    int INCX = 1;
    realno *Y = r.get_data();
    realno *A = data;
    int INCY = 1;
    dgemv_(&trans,&M, &N, &alpha, A, &LDA, X, &INCX, &beta, Y, &INCY);
}

void NagMatrix::output() const
{
    for (unsigned int i = 0; i < rows; i++)
    {
	if (rows > 1)
	{
	    // produce nice matrix bracket
	    if (i == 0)
		cinfo << " / ";
	    else
		if (i == (rows - 1))
		    cinfo << " \\ ";
		else
		    cinfo << " | ";
	}
	else
	    cinfo << " [ ";

	// write row
	for (unsigned int j = 0; j < columns; j++)
	    cinfo << " " << read(i,j) << " ";

	if (rows > 1)
	{
	    // produce nice matrix bracket
	    if (i == 0)
		cinfo << " \\ " << endl;
	    else
		if (i == (rows - 1))
		    cinfo << " / " << endl;
		else
		    cinfo << " | " << endl;
	}
	else
	    cinfo << " ] " << endl;
    }
}

void NagMatrix::get_column(unsigned int c, NagVector &result) const
{
    assert (c < columns);

    if (result.get_data() == NULL)
	result.reconstruct(rows);
    
    assert (result.get_size() >= rows);  // should be after reconstruct

    unsigned int row;
    
    for (row=0; row < rows; row++)
	result.set (row, read (row,c));
}

void NagMatrix::get_row(unsigned int r, NagVector &result) const
{
    assert (r < rows);

    if (result.get_data() == NULL)
	result.reconstruct(columns);

    assert (result.get_size() >= columns);  // should be after reconstruct

    unsigned int col;
    
    for (col=0; col < columns; col++)
	result.set (col,read (r,col));
} 

void NagMatrix::set_column(const unsigned int c, const NagVector &column)
{
    assert (c < columns);
    assert (column.get_size() <= rows);
    
    realno *pnt1 = get(0,c);
    const realno *pnt2 = &(column[column.get_size()-1]);
    const realno *pnt3 = column.get_data_const();

    while (pnt3 <= pnt2)
	*pnt1++ = *pnt3++;
}

void NagMatrix::set_row(const unsigned int r, const NagVector &row)
{
    assert (r < rows);
    assert (row.get_size() <= columns);

    realno *pnt1 = get(r,0);
    const realno *pnt2 = &(row[row.get_size()-1]);
    const realno *pnt3 = row.get_data_const();

    for (;pnt3 <= pnt2; pnt1 += rows)
	*pnt1 = *pnt3++;
}

void NagMatrix::set_block(const unsigned int row_off, const unsigned int col_off, const NagMatrix &block_m)
{
    if (((block_m.rows + row_off) > rows) ||
	(block_m.columns + col_off) > columns)
	matrix_error(" bad call to set_block");
    
    unsigned int i1 = row_off;
    for (unsigned int i2 = 0; i2 < block_m.rows; i2++)
    {
	unsigned int j1 = col_off;
	for (unsigned int j2 = 0; j2 < block_m.columns; j2++)
	    set(i1,j1++,block_m.read(i2,j2));
	
	i1++;
    }
}

void NagMatrix::get_block(const unsigned int row_top, const unsigned int row_bottom,
			  const unsigned int col_left, const unsigned int col_right,
			  NagMatrix &block) const
{
    if (block.data == NULL)
	block.reconstruct(row_bottom - row_top + 1,
			  col_right - col_left + 1);
    
    if (((block.rows + row_top) > rows) ||
	((block.columns + col_left) > columns))
	matrix_error("bad call to get_block");
    
    for (unsigned int i1 = row_top; i1 <= row_bottom; i1++)
	for (unsigned int j1 = col_left; j1 <= col_right; j1++)
	    block.set(i1 - row_top, j1 - col_left, read(i1,j1));
}


void NagMatrix::copy (NagMatrix &res) const
{
    if (res.data == NULL)
	res.reconstruct(rows, columns);
    
    assert (res.no_rows() == rows);
    assert (res.no_columns() == columns);

//    realno *pnt1 = data;
//    realno *pnt2 = get(rows-1, columns-1);
//    realno *pnt3 = res.data;
//
//    while (pnt1 <= pnt2)
//	*pnt3++ = *pnt1++;

    memcpy((void*)res.data, (void*)data, rows*columns*sizeof(realno));
}


void NagMatrix::operator= (const NagMatrix &m2)
{
    if ((data != NULL) && own_memory)
	delete [] data;

    rows = m2.rows;
    columns = m2.columns;
    
    if ((rows*columns) > 0)
    {
	data = new realno[rows*columns];
	own_memory = true;

	memcpy((void*)data, (void*)m2.data, rows * columns * sizeof(realno));
    }
    else
    {
	data = NULL;
	own_memory = false;
    }
}

void NagMatrix::square_entries (NagMatrix &res) const
{
    if (res.data == NULL) res.reconstruct(rows, columns);
    if ((rows != res.rows) || (columns != res.columns))
	matrix_error(" bad call to square_entries");
    realno *pnt1 = data;
    realno *pnt2 = res.data;
    realno *edat = data + (rows * columns);
    for (;pnt1 < edat; pnt1++)
	*pnt2++ = (*pnt1 * *pnt1);
}


void NagMatrix::pseudo_invert (NagMatrix &res) const
{
    if (res.data == NULL) res.reconstruct(columns, rows);
    if ((rows != res.columns) || (columns != res.rows))
	matrix_error(" bad call to pseudo_inverse");
    
    NagMatrix h_t(columns,rows);
    transpose(h_t);
    
    if (rows > columns)
    {
	
	NagMatrix hTh(columns, columns);
	
	h_t.multiply(*this, hTh);
	
	NagMatrix inv_hTh(columns, columns);
	hTh.invert(inv_hTh);
	
	inv_hTh.multiply(h_t, res);
    }
    else
    {
	NagMatrix hhT(rows,rows);
	multiply(h_t, hhT);
	
	NagMatrix inv_hhT(rows,rows);
	hhT.invert(inv_hhT);
	
	h_t.multiply(inv_hhT, res);
    }
    
}

// get_eigenvectors(result, evals))
//  calculates eigenvalues, eigenvectors of real symmetric matrix
//
//  on entry:
//    (*this) real symmetric matrix
//  on exit:
//     result = (v_1 ... v_n) eigenvectors
//      evals = (e_1 ... e_n) eigenvalues in ascending order


void NagMatrix::get_eigenvectors (NagMatrix &result, NagVector &evals) const
{
    if (rows != columns) matrix_error("bad call to get_eigenvectors");
    if (evals.get_data() == NULL) evals.reconstruct(rows);
    if (result.data == NULL) result.reconstruct(rows, rows);
    if ((evals.get_size() != rows) || (result.rows != rows) ||
	(result.columns != columns))
	matrix_error("bad call to get_eigenvectors");
    
#ifdef USE_LAPACK
    copy(result);
    char JOBZ = 'V';       // 'V':  Compute eigenvalues and eigenvectors
    char UPLO = 'U';       // 'U':  Upper triangle of A is stored
    int N = (int) rows;    // The order of the matrix A
    int LDA = (int) rows;
//    int ISPEC = 1;
    //int NB = ilaenv_(&ISPEC, "dsytrd", "U",  &N, &LDA, &minus1, &minus1); 
    int NB = 1;            // number of blocks; NB=1: unblocked algorithm
    int LWORK = (NB+2)*N;
    int INFO;
    NagVector WORK(LWORK);

    dsyev_(&JOBZ, &UPLO, &N, result.data, &LDA, evals.get_data(),
	   WORK.get_data(), &LWORK, &INFO);

    if (INFO != 0)
	matrix_error("could not get eigenvectors");    
#else
    int N = (int) rows;
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    NagVector temp(N);
    int IFAIL = DEF_IFAIL;  // return value: > 0 on failure; value on entry determines error message
    f02abf_ (A.data, &N, &N, evals.data, result.data, &N, temp.data, &IFAIL);

    if (IFAIL != 0)
	matrix_error("failed to get eigensystem");
#endif  
    
}

// get_eigensystem 
// finds the (complex) eigenvalues, eigenvectors
// of an arbitrary square non-symmetric real-valued matrix

void NagMatrix::get_eigensystem (NagMatrix &res_r,  // output: right eigenvectors, real part
				 NagMatrix &res_i,  // output: right eigenvectors, imag part
				 NagVector &evals_r,
				 NagVector &evals_i) const
{
    assert (rows == columns);    // otherwise bad call to get_eigensystem (non-square matrix)

    if (evals_r.get_data() == NULL) evals_r.reconstruct(rows);
    if (evals_i.get_data() == NULL) evals_i.reconstruct(rows);
    if (res_r.data == NULL) res_r.reconstruct(rows, rows);
    if (res_i.data == NULL) res_i.reconstruct(rows, rows);
    
    // make sure matrix dimensions are sufficient to avoid overflow...
    assert (res_r.no_rows() >= rows);
    assert (res_r.no_columns() >= rows);
    assert (res_i.no_rows() >= rows);
    assert (res_i.no_columns() >= rows);
    assert (evals_r.get_size() >= rows);
    assert (evals_i.get_size() >= rows);

#ifdef USE_LAPACK
    NagMatrix A (*this);         // make a copy because the BLAS/LAPACK routine modifies A.

    cinfo << "WARNING: NagMatrix::get_eigensystem() does not seem functional with LAPACK." << endl;

    // dgeev settings...
    int N = (int) rows;
    int LWORK = 4 * N;           // workspace size
//    if (best_work > LWORK)
//        LWORK = best_work;        // use optimal workspace size determined last time
    char JOBVL = 'N';            // left eigenvectors are not to be computed
    char JOBVR = 'V';            // right eigenvectors are to be computed
    int LDA = N;                 // leading dimension of A is N
    int LDVL = 1;                // leading dimension of left eigenvectors matrix, >= 1
    int LDVR = res_r.no_rows();  // leading dimension of right eigenvectors matrix, >= N
    
    NagVector WORK(LWORK);       // workspace

    if (WORK.get_data() == NULL)
	matrix_error("couldn't assign workspace");

    int INFO;
    
    dgeev_(&JOBVL, &JOBVR, &N, A.data, &LDA, evals_r.get_data(),
	   evals_i.get_data(), NULL, &LDVL, res_r.data, &LDVR, WORK.get_data(),
	   &LWORK, &INFO);
    
    if (INFO != 0)
	matrix_error("could not get eigenvectors");
//    else    
//	best_work = (int) WORK[1]; // save optimal workspace size


    // unpack the complex eigenvectors
    NagVector zero_v(N);
    zero_v.clear();
    NagVector tmp_v;

    for (unsigned int j = 0; j < N; j++)
    {
	if (evals_i[j] == 0)
	    res_i.set_column(j, zero_v);
	else
	{
	    res_r.get_column(j+1, tmp_v);
	    res_i.set_column(j, tmp_v);
	    tmp_v *= -1.0;
	    res_i.set_column(j+1, tmp_v);
	    
	    res_r.get_column(j, tmp_v);
	    res_r.set_column(j+1, tmp_v);
	    
	    j++;
	}
    }
    
#else
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    int IA = (int) rows;
    int N = (int) rows;
    int IVR = (int) res_r.no_rows();
    int IVI = (int) res_i.no_rows();
    int *INTGER = new int[N];
    int IFAIL = DEF_IFAIL;  // return value: > 0 on failure, value on entry determines error message
    f02agf_(A.data, &IA, &N, evals_r.data, evals_i.data,
	    res_r.data, &IVR, res_i.data, &IVI, INTGER, &IFAIL);
    delete [] INTGER;