nagmatrix.cc

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

#endif  
    
}

void NagMatrix::QR_factorise(NagMatrix &Q) const
{
#ifdef USE_LAPACK
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    int M = (int) rows;
    int N = (int) columns;
    if (Q.data == NULL) Q.reconstruct(M,M);
    int LDA = M;
    int K = N;
    int INFO;
    int NB=4;

    int LWORK =NB*N;
    NagVector WORK(LWORK);
    NagVector zeta(N);
   
    dgeqrf_(&M, &N, A.data, &LDA, zeta.get_data(), WORK.get_data(), &LWORK, &INFO );

    if (INFO != 0) matrix_error(" can't QR factorise");

    dorgqr_(&M, &N, &K, A.data, &LDA, zeta.get_data(), WORK.get_data(), &LWORK, &INFO );

    if (INFO != 0) matrix_error(" can't QR factorise");

    A.copy(Q);

#else
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    int M = (int) rows;
    int N = (int) columns;
    if (Q.data == NULL) Q.reconstruct(M,M);
    int LDA = M;
    int K = N;
    NagVector zeta(N);
    NagVector WORK(K);
    int IFAIL = DEF_IFAIL;  // return value: > 0 on failure, value on entry determines error message

    f01qcf_(&M, &N, A.data, &LDA, zeta.data, &IFAIL);

    IFAIL = DEF_IFAIL;      // re-set

    f01qef_("S", &M, &N, &K, A.data, &LDA, zeta.data, WORK.data, &IFAIL);
    A.copy(Q);
#endif    
}


// msqrt
// returns the positive definite square root
// of a real symmetric matrix

void NagMatrix::msqrt(NagMatrix &res)
{
    if (rows != columns)
	matrix_error("bad call to NagMatrix::msqrt");
    
    NagMatrix evecs;
    NagVector evals;
    
    get_eigenvectors(evecs, evals);
    NagMatrix lambda_sqrt(rows, rows, 0.0);
    for (unsigned int i = 0; i < rows; i++)
	*lambda_sqrt.get(i,i) = sqrt(evals[i]);
    NagMatrix evecs_t;  
    evecs.transpose(evecs_t);
    
    NagMatrix tmp;
    lambda_sqrt.multiply(evecs_t, tmp);
    evecs.multiply(tmp, res);
}


// sv_decompose
// Singular Value Decomposition
// A =  Q D P'
// where Q , P are orthogonal matrices

void NagMatrix::sv_decompose(NagMatrix &Q, NagVector &sv,
			     NagMatrix &P_t) const
{
#ifdef USE_LAPACK
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.

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

    int M = (int) rows;
    int N = (int) columns;
    int LDA = M;
    int LDU=M;                     
    int LDVT=N;
    int INFO;
    int min_mn = MIN(M,N);

    int LWORK=MAX(3*MIN(M,N)+MAX(M,N),5*MIN(M,N));
    NagVector WORK(LWORK);
  
    if (Q.data == NULL)Q.reconstruct(M,min_mn);             
    if (sv.get_data() == NULL) sv.reconstruct(min_mn);
    if (P_t.data == NULL) P_t.reconstruct(min_mn,N);

    dgesvd_( "A", "A", &M, &N, A.data,&LDA, sv.get_data(), Q.data, &LDU, P_t.data, &LDVT,
	                        WORK.get_data(),&LWORK, &INFO );

    if (INFO != 0) matrix_error(" can't sv_decompose");
     
    if (M == N) A.copy(Q);
    if (M > N) A.get_block(0,M-1,0, N-1, Q);
    if (M < N) A.get_block(0,M-1,0, N-1, P_t);

#else
    int M = (int) rows;
    int N = (int) columns;
    int LDA = M;
    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    int NCOLB = 0;
    int LDB = 0;
    int WANTQ = 1;
    int min_mn = MIN(M,N);
    if (Q.data == NULL) 
	Q.reconstruct(M,min_mn);
    int LDQ = M;
    if (sv.data == NULL) sv.reconstruct(min_mn);
    int WANTP = 1;
    if (P_t.data == NULL) P_t.reconstruct(min_mn,N);
    int LDPT = N;
    NagVector WORK(MAX(N * N + 5 * (N - 1), N + 4));
    
    int IFAIL = DEF_IFAIL;  // return value: > 0 on failure, value on entry determines error message
    f02wef_(&M, &N, A.data, &LDA, &NCOLB, NULL, &LDB, &WANTQ, Q.data,
	    &LDQ, sv.data, &WANTP, P_t.data, &LDPT, WORK.data, &IFAIL);
    
    if (M == N) A.copy(Q);
    if (M > N) A.get_block(0,M-1,0, N-1, Q);
    if (M < N) A.get_block(0,M-1,0, N-1, P_t);
#endif
    
}


// ortho transform
// if flag is true (default)
// 		calculates Qt A Q
// else
//		calculates Q A Qt
//
// (where Qt is Q transform)
void NagMatrix::ortho_transform(NagMatrix &Q, NagMatrix &res, const bool flag) const
{
    NagMatrix Q_t;
    NagMatrix tmp;
    Q.transpose(Q_t);
    if (flag)
    {
	multiply(Q, tmp);
	Q_t.multiply(tmp, res);
    }
    else
    {
	multiply(Q_t, tmp);
	Q.multiply(tmp, res);
    }
}

ostream &operator<<(ostream &out_s, const NagMatrix &m)
{
    out_s << m.rows << " by " << m.columns << " matrix" << endl;
    /*  NagVector col(m.rows);
	for (int i = 0; i < m.columns; i++)
	{
	m.get_column(i,col);
	out_s << "column " << i << endl << col << endl;
	} */
    NagVector v(m);
    out_s << v;
//    m.reconstruct(m.rows, m.columns, v);
    return out_s;
}

istream &operator>>(istream &in_s, NagMatrix &m)
{
    char dummy[50];
    unsigned int r,c;
    in_s >> r >> dummy >> c >> dummy;
    
    if (m.data == NULL)
	m.reconstruct(r,c);
    else
	if ((r != m.rows) || (c != m.columns)) 
	    m.matrix_error("wrong sized matrix");
    
    /*
      NagVector col(m.rows);
      for (unsigned int i = 0; i < m.columns; i++)
      {
      in_s >> dummy >> dummy >> col;
      m.set_column(i,col);
      }*/
    
    NagVector v(m);
    in_s >> v; 
    m.reconstruct(r,c,v);
    return in_s;
}

void NagMatrix::pack_to_triangle(NagVector &res) const
{
    unsigned int N = MIN(rows, columns);

    if (res.get_data() == NULL)
	res.reconstruct(N * (N+1) / 2);
    
    if (res.get_size() != (N * (N+1) / 2))
	matrix_error("bad call to pack_to_triangle");
    
    realno *pnt1 = res.get_data();
    for (unsigned int i = 0; i < N; i++)
	for (unsigned int j = 0; j <= i; j++)
	    *pnt1++ = read(i,j);
    
}

void NagMatrix::unpack_triangle(const NagVector &dat, bool symmetric)
{
    if (data == NULL)
    {
	unsigned int N = (int)
	    (sqrt(2 * dat.get_size() + 0.25) - 0.49999);
	reconstruct(N,N);
    }
    
    unsigned int N = MIN(rows, columns);
    if (dat.get_size() != (N * (N+1) / 2))
	matrix_error("bad call to unpack_triangle");
    
    const realno *pnt1 = dat.get_data_const();
    if (symmetric)
    {
	for (unsigned int i = 0; i < N; i++)
	    for (unsigned int j = 0; j <= i; j++)
		*get(i,j) = *get(j,i) = *pnt1++;
    }
    else 
    {
	for (unsigned int i = 0; i < N; i++)
	    for (unsigned int j = 0; j <= i; j++)
	    {
		*get(j,i) = 0;
		*get(i,j) = *pnt1++;
	    }
    }
}

void NagMatrix::lower_to_full(NagMatrix &X_full)
{
    // X_full = X_lower + (X_lower)^T
    NagMatrix X_t;
    transpose(X_t);
    add(X_t, X_full);
}

void NagMatrix::scale_diagonal(realno fac)
{
    if (data == NULL)
	matrix_error("bad call to scale_diagonal");
    
    unsigned int N = MIN(rows, columns);
    for (unsigned int i = 0; i < N; i++)
	*get(i,i) *= fac;
}

realno NagMatrix::trace()
{
    realno res = 0;
    unsigned int N = MIN(rows, columns);
    for (unsigned int i = 0; i < N; i++)
	res += *get(i,i);
    
    return res;
}


void NagMatrix::read_diagonal(NagVector &diag)
{
    unsigned int N = MIN(rows, columns);
    if (diag.get_data() == NULL) diag.reconstruct(N);
    if (diag.get_size() != N)
	matrix_error("bad call to read_diagonal");
    
    for (unsigned int i = 0; i < N; i++)
	diag.set (i, read(i,i));
    
}

void NagMatrix::set_diagonal(NagVector &diag)
{
    unsigned int N = MIN(rows, columns);
    N = MIN(N, diag.get_size());
    for (unsigned int i = 0; i < N; i++)
	set(i,i,diag[i]);
    
}
realno NagMatrix::trace_A_Bt(NagMatrix &A, NagMatrix &B)
{
    if ((A.rows != B.rows) || (A.columns != B.columns))
	matrix_error("bad call to trace_A_Bt");
    
    realno res = 0.0;
    for (unsigned int i = 0; i < A.rows; i++)
	for (unsigned int j = 0; j < A.columns; j++)
	    res += A.read(i,j) * B.read(i,j);
    
    return res;
}

realno NagMatrix::trace_A_B(NagMatrix &A, NagMatrix &B)
{
    if ((A.rows != B.columns) || (A.columns != B.rows))
	matrix_error("bad call to trace_A_Bt");
    
    realno res = 0.0;
    for (unsigned int i = 0; i < A.rows; i++)
	for (unsigned int j = 0; j < A.columns; j++)
	    res += A.read(i,j) * B.read(j,i);
    
    return res;
}

// solves A * x  = b with A an M x N matrix, b is M x 1
void NagMatrix::solve_equations(NagMatrix b, NagMatrix &x) const
{
#ifdef USE_LAPACK
    int N = (int) MAX(rows, columns);
    int M = (int) b.columns;
    int INFO;

    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.

    if (x.data == NULL)
	x.reconstruct(N, b.columns);

    if ((x.rows != N) || (x.columns != M) || (b.rows != N))
	matrix_error("bad call to solve_equations");
    
    int IA = (int) rows;
    int IB = (int) columns;
    int LWORK = MIN(rows, columns) + MAX((int) MIN(rows, columns), M);  // workspace size

    NagVector WORK(LWORK);
    
    dgels_( "N", &IA, &IB, &M, A.data, &IA, b.data, &N, WORK.get_data(), &LWORK, &INFO);

    if (INFO != 0)
	matrix_error(" failed to solve equation");

    b.copy(x);

#else

    NagMatrix A (*this);    // make a copy because the NAG routine modifies A.
    
    int M = (int) rows;
    int N = (int) columns;
    int NRA = (int) rows;
    
    // tolerance: correctness of the elements of A
    realno TOL = 0.000005;  //  e.g. 5e-4 means correct to about 4 significant digits
    bool SVD;               // return value: whether A is of full rank
    realno SIGMA;           // return value: the standard error
    int IRANK;              // return value: the rank of A, if SVD == false
    int IFAIL = DEF_IFAIL;  // return value: > 0 on failure, value on entry determines error message
    
    int LWORK = 4 * N;
    realno WORK[LWORK];
    
    if (x.data == NULL)
	x.reconstruct(N, b.columns);

    if ((M < N) || (x.rows != N) || (x.columns != 1) || (b.rows != M))
	matrix_error("bad call to solve_equations");

    // f04jgf also solves overdetermined systems, giving the least squares solution.
    
    f04jgf_(&M, &N, A.data, &NRA, b.data, &TOL, &SVD, &SIGMA, &IRANK, &WORK[0], &LWORK, &IFAIL);

    if (IFAIL != 0)
	matrix_error("failed call to solve_equations");

    // copy over the result from b to x
    for (unsigned int column = 0; column < b.columns; column++)
	for (unsigned int row = 0; row < N; row++)
	    x.set(row, column, b.read(row, column));

#endif
    
}


void NagMatrix::flip_horizontally()
{
    NagVector c1, c2;
    unsigned int imax = (columns / 2);
    for (unsigned int i = 0; i < imax; i++)
    {
	get_column(i, c1);
	get_column(columns - i - 1, c2);
	set_column(i, c2);
	set_column(columns - i - 1, c1);
    }
}

} // namespace ReadingPeopleTracker