bool robManipulator::JacobianSpatial(const vctDynamicVector<double>& q,
vctDynamicMatrix<double>& J) const
{
JacobianSpatial(q);
if ((J.rows() != 6) && (J.cols() != links.size()))
return false;
for (size_t r = 0; r < 6; r++)
for (size_t c = 0; c < links.size(); c++)
J.Element(r,c) = Js[c][r];
return true;
}
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];
}
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;
}
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;
}
osaPDGC::osaPDGC( const std::string& robfile,
const vctFrame4x4<double>& Rtw0,
const vctDynamicMatrix<double>& Kp,
const vctDynamicMatrix<double>& Kd,
const vctDynamicVector<double>& qinit ):
robManipulator( robfile, Rtw0 ),
Kp( Kp ),
Kd( Kd ),
qold( qinit ),
eold( qinit.size(), 0.0 ) {
if( Kp.rows() != links.size() || Kp.cols() != links.size() ) {
CMN_LOG_RUN_ERROR << "size(Kp) = [" << Kp.rows()
<< ", " << Kp.cols() << " ] "
<< "NxN = [" << links.size()
<< ", " << links.size() << " ]"
<< std::endl;
}
if( Kd.rows() != links.size() || Kd.cols() != links.size() ) {
CMN_LOG_RUN_ERROR << "size(Kd) = [" << Kd.rows()
<< ", " << Kd.cols() << " ] "
<< "NxN = [" << links.size()
<< ", " << links.size() << " ]"
<< std::endl;
}
if( qold.size() != links.size() ) {
CMN_LOG_RUN_ERROR << "size(qold) = " << qold.size() << " "
<< "N = " << links.size() << std::endl;
}
}
nmrSymmetricEigenProblem::Errno
nmrSymmetricEigenProblem
( vctDynamicMatrix<double>& A,
vctDynamicVector<double>& D,
vctDynamicMatrix<double>& V,
nmrSymmetricEigenProblem::Data& data ){
// check if we need to reallocate data
if( ( data.JOBZ == 'N' && data.RANGE == 'U' && data.UPLO == 'L' ) ||
data.N != int(A.rows()) ||
data.ISUPPZ == NULL ||
data.WORK == NULL ||
data.IWORK == NULL ){
data.Free();
data = nmrSymmetricEigenProblem::Data( A, D, V );
}
dsyevr_( &data.JOBZ, &data.RANGE, &data.UPLO,
&data.N, data.A, &data.LDA,
&data.VL, &data.VU,
&data.IL, &data.IU,
&data.ABSTOL,
&data.M, data.W,
data.Z, &data.LDZ, data.ISUPPZ,
data.WORK, &data.LWORK,
data.IWORK, &data.LIWORK,
&data.INFO );
if( data.INFO == 0 ) { return nmrSymmetricEigenProblem::ESUCCESS; }
return nmrSymmetricEigenProblem::EFAILURE;
}
void
robManipulator::AddIdentificationColumn
( vctDynamicMatrix<double>& J,
vctFixedSizeMatrix<double,4,4>& delRt ) const{
size_t c = J.cols();
J.resize( 6, c+1 );
J[0][c] = delRt[0][3]; // dx
J[1][c] = delRt[1][3]; // dy
J[2][c] = delRt[2][3]; // dz
J[3][c] = (delRt[2][1] - delRt[1][2])/2.0; // average wx
J[4][c] = (delRt[0][2] - delRt[2][0])/2.0; // average wy
J[5][c] = (delRt[1][0] - delRt[0][1])/2.0; // average wz
}
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;
}
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;
}
nmrLSMinNorm::Data::Data( const vctDynamicMatrix<double>& A,
const vctDynamicMatrix<double>& b,
double rcond ) :
M( A.rows() ),
N( A.cols() ),
NRHS( b.cols() ),
LDA( M ),
LDB( (M<N) ? N : M ),
S( new CISSTNETLIB_DOUBLE[ (M<N) ? M : N ] ),
RCOND( rcond ),
WORK( NULL ),
LWORK( -1 ), // -1 to determined the optimal work space size
INFO( 0 ),
underdetermined( (M<N) ? true : false ){
CheckSystem( A, b );
// this call determines the optimal work space size
CISSTNETLIB_DOUBLE work[1]; // size will be here
dgelss_( &M, &N, &NRHS,
NULL, &LDA,
NULL, &LDB,
&S[0], &RCOND, &RANK,
&work[0], &LWORK, &INFO );
LWORK = work[0]; // copy the work space size
WORK = new CISSTNETLIB_DOUBLE[LWORK]; // allocate the work space
// if system is underdetermined (M<N) we need a larger b matrix
if( underdetermined ){
B.SetSize( LDB, NRHS, VCT_COL_MAJOR );
for( int r=0; r<M; r++ ){
for( int c=0; c<NRHS; c++ ){
B[r][c] = b[r][c];
}
}
}
}
osaOpenNI::Errno
osaOpenNI::Convert3DToProjectiveMask(const vctDynamicMatrix<double>& rangedata,
vctDynamicMatrix<bool>& mask) {
// allocate the arrays
XnUInt32 cnt = rangedata.cols();
XnPoint3D* wrld = new XnPoint3D[ cnt ];
XnPoint3D* proj = new XnPoint3D[ cnt ];
// copy the 3d points to the 3d array
for (XnUInt32 i=0; i<cnt; i++) {
wrld[i].X = -rangedata[0][i]*1000.0;
wrld[i].Y = rangedata[1][i]*1000.0;
wrld[i].Z = rangedata[2][i]*1000.0;
}
// convert to projective
XnStatus status;
status = Data->depthgenerator.ConvertRealWorldToProjective(cnt, wrld, proj);
if (status != XN_STATUS_OK) {
CMN_LOG_RUN_ERROR << "Failed to convert world to projective: "
<< xnGetStatusString(status) << std::endl;
return osaOpenNI::EFAILURE;
}
// use this to find the size of the mask
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
mask.SetSize(depthMD.YRes(), depthMD.XRes());
mask.SetAll(false);
for (XnUInt32 i=0; i<cnt; i++) {
mask[ proj[i].Y ][ proj[i].X ] = true;
}
delete[] wrld;
delete[] proj;
return osaOpenNI::ESUCCESS;
}
void nmrLSMinNorm::Data::CheckSystem( const vctDynamicMatrix<double>& A,
const vctDynamicMatrix<double>& b ) const{
// test that number of rows match
if( A.rows() != b.rows() ){
std::ostringstream message;
message << "nmrLSMinNorm: Matrix A has " << A.rows() << " rows. "
<< "Matrix B has " << b.rows() << " rows.";
cmnThrow( std::runtime_error( message.str() ) );
}
// check for fortran format
if( !( A.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix A format." );
cmnThrow( std::runtime_error( message ) );
}
if( !( b.IsFortran() ) ){
std::string message( "nmrLSMinNorm: Invalid matrix b format." );
cmnThrow( std::runtime_error( message ) );
}
}
void
nmrSymmetricEigenProblem::Data::CheckSystem
( const vctDynamicMatrix<double>& A,
const vctDynamicVector<double>& D,
const vctDynamicMatrix<double>& V ){
// test that number of rows match
if( A.rows() != A.cols() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Matrix A has " << A.rows() << " rows "
<< " and " << A.cols() << " columns.";
cmnThrow( std::runtime_error( message.str() ) );
}
// check for fortran format
if( !A.IsFortran() ){
std::string message( "nmrSymmetricEigenProblem: Invalid matrix A format." );
cmnThrow( std::runtime_error( message ) );
}
// test that number of rows match
if( V.rows() != A.rows() || V.cols() != A.cols() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Matrix V has " << V.rows() << " rows "
<< " and " << V.cols() << " columns.";
cmnThrow( std::runtime_error( message.str() ) );
}
if( !V.IsFortran() ){
std::string message( "nmrSymmetricEigenProblem: Invalid matrix V format." );
cmnThrow( std::runtime_error( message ) );
}
if( D.size() != A.rows() ){
std::ostringstream message;
message << "nmrSymmetricEigenProblem: Vector D has " << D.size() << " rows";
cmnThrow( std::runtime_error( message.str() ) );
}
}
osaOpenNI::Errno
osaOpenNI::GetRangeData(vctDynamicMatrix<double>& rangedata,
const std::vector< vctFixedSizeVector<unsigned short,2> >& pixels) {
// Get data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
// create arrays
XnUInt32 cnt;
XnPoint3D* proj = NULL;
XnPoint3D* wrld = NULL;
if (pixels.empty()) {
// create arrays
cnt = depthMD.XRes()*depthMD.YRes();
proj = new XnPoint3D[ cnt ];
wrld = new XnPoint3D[ cnt ];
// Create projective coordinates
for (size_t i=0, x=0; x<depthMD.XRes(); x++) {
for (size_t y=0; y<depthMD.YRes(); i++, y++) {
proj[i].X = (XnFloat)x;
proj[i].Y = (XnFloat)y;
proj[i].Z = depthMD(x,y);
}
}
}
else{
// create arrays
cnt = pixels.size();
proj = new XnPoint3D[ cnt ];
wrld = new XnPoint3D[ cnt ];
for (size_t i=0; i<pixels.size(); i++) {
unsigned int x = pixels[i][0];
unsigned int y = pixels[i][1];
proj[i].X = (XnFloat)x;
proj[i].Y = (XnFloat)y;
proj[i].Z = depthMD(x,y);
}
}
// Convert projective to 3D
XnStatus status = Data->depthgenerator.ConvertProjectiveToRealWorld(cnt, proj, wrld);
if (status != XN_STATUS_OK) {
CMN_LOG_RUN_ERROR << "Failed to convert projective to world: "
<< xnGetStatusString(status) << std::endl;
}
// create matrix
rangedata.SetSize(3, cnt);
for (size_t i=0; i<cnt; i++) {
rangedata[0][i] = -wrld[i].X/1000.0;
rangedata[1][i] = wrld[i].Y/1000.0;
rangedata[2][i] = wrld[i].Z/1000.0;
}
delete[] proj;
delete[] wrld;
return osaOpenNI::ESUCCESS;
}
