nmrSymmetricEigenProblem::Data::Data( vctDynamicMatrix<double>& A,
vctDynamicVector<double>& D,
vctDynamicMatrix<double>& V ) :
JOBZ( 'V' ),
RANGE( 'A' ),
UPLO( 'U' ),
N( A.rows() ),
A( A.Pointer() ),
LDA( A.cols() ),
VL( 0 ),
VU( 0 ),
IL( 0 ),
IU( 0 ),
DLAMCH( 'S' ),
ABSTOL( dlamch_( &DLAMCH ) ),
W( D.Pointer() ),
Z( V.Pointer() ),
LDZ( V.cols() ),
ISUPPZ( new CISSTNETLIB_INTEGER[ 2*N ] ),
WORK( NULL ),
LWORK( -1 ),
IWORK( NULL ),
LIWORK( -1 ){
CheckSystem( A, D, V );
CISSTNETLIB_DOUBLE work;
CISSTNETLIB_INTEGER iwork;
dsyevr_( &JOBZ, &RANGE, &UPLO,
&N, this->A, &LDA,
&VL, &VU,
&IL, &IU,
&ABSTOL,
&M, W,
Z, &LDZ, ISUPPZ,
&work, &LWORK,
&iwork, &LIWORK,
&INFO );
LWORK = work;
WORK = new CISSTNETLIB_DOUBLE[LWORK];
LIWORK = iwork;
IWORK = new CISSTNETLIB_INTEGER[LIWORK];
}
vctDynamicMatrix<double> nmrLSMinNorm( vctDynamicMatrix<double>& vctA,
vctDynamicMatrix<double>& vctb,
nmrLSMinNorm::Data& data,
CISSTNETLIB_DOUBLE rcond ){
// data pointers
CISSTNETLIB_DOUBLE* A = vctA.Pointer();
CISSTNETLIB_DOUBLE* B = vctb.Pointer();
// check if we need to reallocate data
if( data.M != int(vctA.rows()) ||
data.N != int(vctA.cols()) ||
data.NRHS != int(vctb.cols()) ){
if( data.S != NULL ) { delete[] data.S; }
if( data.WORK != NULL ) { delete[] data.WORK; }
data = nmrLSMinNorm::Data( vctA, vctb, rcond );
}
// copy the data for underdetermined systems
if( data.underdetermined ){ B = data.B.Pointer(); }
// solve the LS with minimum norm
dgelss_( &data.M, &data.N, &data.NRHS,
&A[0], &data.LDA,
&B[0], &data.LDB,
&data.S[0], &data.RCOND, &data.RANK,
&data.WORK[0], &data.LWORK, &data.INFO );
data.CheckInfo();
// Assign???
vctDynamicMatrix<double> vctx( data.N, data.NRHS, VCT_COL_MAJOR );
if( data.underdetermined ){
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = data.B[r][c];
}
}
}
else{
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = vctb[r][c];
}
}
}
return vctx;
}
vctDynamicMatrix<double> nmrLSMinNorm( vctDynamicMatrix<double>& vctA,
vctDynamicMatrix<double>& vctb,
CISSTNETLIB_DOUBLE rcond ){
// data pointers
CISSTNETLIB_DOUBLE* A = vctA.Pointer();
CISSTNETLIB_DOUBLE* B = vctb.Pointer();
// allocate data. Allocate a LDBxNRHS B matrix for underdetermined systems.
nmrLSMinNorm::Data data( vctA, vctb, rcond );
// copy the data for underdetermined systems
if( data.underdetermined ){ B = data.B.Pointer(); }
// solve the LS with minimum norm
dgelss_( &data.M, &data.N, &data.NRHS,
&A[0], &data.LDA,
&B[0], &data.LDB,
&data.S[0], &data.RCOND, &data.RANK,
&data.WORK[0], &data.LWORK, &data.INFO );
delete[] data.S;
delete[] data.WORK;
data.CheckInfo();
// Assign???
vctDynamicMatrix<double> vctx( data.N, data.NRHS, VCT_COL_MAJOR );
if( data.underdetermined ){
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = data.B[r][c];
}
}
}
else{
for( int r=0; r<data.N; r++ ){
for( int c=0; c<data.NRHS; c++ ){
vctx[r][c] = vctb[r][c];
}
}
}
return vctx;
}
osaOpenNI::Errno osaOpenNI::GetRGBImage(vctDynamicMatrix<unsigned char>& RGBimage) {
xn::ImageMetaData rgbMD;
Data->rgbgenerator.GetMetaData(rgbMD);
// create image
RGBimage.SetSize(rgbMD.YRes(), rgbMD.XRes()*3);
memcpy(RGBimage.Pointer(), rgbMD.Data(), rgbMD.YRes()*rgbMD.XRes()*3*sizeof(unsigned char));
return osaOpenNI::ESUCCESS;
}
void osaOpenNI::GetDepthImage(vctDynamicMatrix<double>& placeHolder) {
// Get depth data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
const XnDepthPixel* pDepth = depthMD.Data();
placeHolder.SetSize(depthMD.YRes(), depthMD.XRes());
double* ptr = placeHolder.Pointer();
const size_t end = depthMD.YRes()*depthMD.XRes();
for (size_t i = 0; i < end; i++)
{
(*ptr) = 255.0 * (*pDepth) / 2048.0;
ptr++; pDepth++;
}
}
osaOpenNI::Errno osaOpenNI::GetDepthImageRaw(vctDynamicMatrix<double> & depthimage) {
// Get data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
const XnDepthPixel* src = depthMD.Data();
depthimage.SetSize(depthMD.YRes(), depthMD.XRes());
double* dest = depthimage.Pointer();
const size_t N = depthMD.YRes()*depthMD.XRes();
for (size_t i = 0; i<N; i++) {
(*dest) = (*src);
src++;
dest++;
}
return osaOpenNI::ESUCCESS;
}