//----------------------------------------------------------------------------- int MGL_EXPORT mgl_datac_read_mat(HADT d, const char *fname, long dim) { if(dim<=0 || dim>3) return false; gzFile fp = gzopen(fname,"r"); if(!fp) return false; long nx=1, ny=1, nz=1; char *buf = mgl_read_gz(fp); long nb = strlen(buf); gzclose(fp); register long j=0,i,l; while(j<nb) { if(buf[j]=='#') while(!isn(buf[j])) j++; // skip comment while(buf[j]<=' ' && j<nb) j++; break; } if(dim==1) { sscanf(buf+j,"%ld",&nx); while(buf[j]!='\n' && j<nb) j++; j++; // while(buf[j]>' ') j++; } else if(dim==2) { sscanf(buf+j,"%ld%ld",&nx,&ny); while(buf[j]!='\n' && j<nb) j++; j++; char *b=buf+j, ch; for(i=l=0;b[i];i++) { while(b[i]=='#') { while(!isn(b[i]) && b[i]) i++; } if(b[i]=='\n') l++; } if(l==nx*ny || l==nx*ny+1) // try to read 3d data (i.e. columns of matrix nx*ny) { nz=ny; ny=nx; nx=1; bool first = false; for(i=l=0;b[i] && !isn(b[i]);i++) // determine nx { while(b[i]=='#') { while(!isn(b[i]) && b[i]) i++; } ch = b[i]; if(ch>' ' && !first) first=true; if(first && (ch=='\t' || ch==';') && b[i+1]!='\t') nx++; } } } else if(dim==3) { sscanf(buf+j,"%ld%ld%ld",&nx,&ny,&nz); while(buf[j]!='\n' && j<nb) j++; j++; } mglFromStr(d,buf+j,nx,ny,nz); free(buf); return true; }
//----------------------------------------------------------------------------- int MGL_EXPORT mgl_datac_read(HADT d, const char *fname) { long l=1,m=1,k=1; long nb,i; gzFile fp = gzopen(fname,"r"); if(!fp) { if(!d->a) mgl_datac_create(d, 1,1,1); return false; } char *buf = mgl_read_gz(fp); nb = strlen(buf); gzclose(fp); bool first=false; // space is not allowed delimiter for file with complex numbers register char ch; for(i=nb-1;i>=0;i--) if(buf[i]>' ') break; buf[i+1]=0; nb = i; // remove tailing spaces for(i=0;i<nb-1 && !isn(buf[i]);i++) // determine nx { while(buf[i]=='#') { while(!isn(buf[i]) && i<nb) i++; } ch = buf[i]; if(ch>' ' && !first) first=true; if(first && (ch=='\t' || ch==';') && buf[i+1]!='\t') k++; // ',' is not valid delimiter for complex arrays } first = false; for(i=0;i<nb-1;i++) // determine ny { ch = buf[i]; if(ch=='#') while(!isn(buf[i]) && i<nb) i++; if(isn(ch)) { while(buf[i+1]=='\t') i++; if(isn(buf[i+1])) {first=true; break; } m++; } if(ch=='\f') break; } if(first) for(i=0;i<nb-1;i++) // determine nz { ch = buf[i]; if(ch=='#') while(!isn(buf[i]) && i<nb) i++; // if(ch=='#') com = true; // comment if(isn(ch)) { // if(com) { com=false; continue; } while(buf[i+1]=='\t') i++; if(isn(buf[i+1])) l++; } } else for(i=0;i<nb-1;i++) if(buf[i]=='\f') l++; mglFromStr(d,buf,k,m,l); free(buf); return true; }
/** * * Exit codes: * 0 normal * 1 not enough arguments * 2 invalid device number * 3 invalid format number * 4 XML file not found * 5 Unicap error * 6 Running main loop without a camera **/ int main(int argc, char* argv[]) { ros::init(argc, argv, "image_stream_node"); ImageStreamNode isn(argc, argv); //std::cout << "[DEBUG] Advertising services" << std::endl; ros::NodeHandle nodeHandle; isn.run(); return 0; }
//----------------------------------------------------------------------------- void mglFromStr(HADT d,char *buf,long NX,long NY,long NZ) // TODO: add multithreading read { if(NX<1 || NY <1 || NZ<1) return; mgl_datac_create(d, NX,NY,NZ); long nb = strlen(buf); register long i=0, j=0; setlocale(LC_NUMERIC, "C"); while(j<nb) { while(buf[j]<=' ' && j<nb) j++; while(buf[j]=='#') // skip comment { if(i>0 || buf[j+1]!='#') // this is columns id while(!isn(buf[j]) && j<nb) j++; else { while(!isn(buf[j]) && j<nb) { if(buf[j]>='a' && buf[j]<='z') d->id.push_back(buf[j]); j++; } } while(buf[j]<=' ' && j<nb) j++; } char *s=buf+j; while(buf[j]>=' ' && buf[j]!=';' && j<nb) j++; buf[j]=0; double re=0,im=0; size_t ll=strlen(s); if(*s=='(') sscanf(s,"(%lg,%lg)",&re,&im); else if(*s=='[') sscanf(s,"[%lg,%lg]",&re,&im); else if(*s=='{') sscanf(s,"{%lg,%lg}",&re,&im); else if(s[ll]=='i') { s[ll] = 0; sscanf(s,"%lg+%lg)",&re,&im); } else sscanf(s,"%lg+i%lg",&re,&im); d->a[i] = dual(re,im); i++; if(i>=NX*NY*NZ) break; } setlocale(LC_NUMERIC, ""); }
std::vector<int> reorder(MatrixType const & matrix, gibbs_poole_stockmeyer_tag) { std::size_t n = matrix.size(); std::vector<int> r; std::vector< std::vector<int> > rl; std::size_t l = 0; int state; bool state_end; std::vector< std::vector<int> > nodes; std::vector<bool> inr(n, false); std::vector<bool> isn(n, false); std::vector<int> tmp(2); int g = 0; int h = 0; std::vector< std::vector<int> > lg; std::vector< std::vector<int> > lh; std::vector< std::vector<int> > ls; std::map< int, std::vector<int> > lap; std::vector<int> rg; std::vector< std::vector<int> > rgc; int m; int m_min; bool new_g = true; int k1, k2, k3, k4; std::vector<int> wvs; std::vector<int> wvsg; std::vector<int> wvsh; int deg_min; int deg; int ind_min; r.reserve(n); nodes.reserve(n); while (r.size() < n) // for all components of the graph apply GPS algorithm { // determine node g with mimimal degree among all nodes which // are not yet in result array r deg_min = -1; for (std::size_t i = 0; i < n; i++) { if (!inr[i]) { deg = matrix[i].size() - 1; // node degree if (deg_min < 0 || deg < deg_min) { g = i; // node number deg_min = deg; } } } // algorithm for determining nodes g, h as endpoints of a pseudo graph diameter while (new_g) { lg.clear(); detail::generate_layering(matrix, lg, g); nodes.resize(0); for (std::size_t i = 0; i < lg.back().size(); i++) { tmp[0] = lg.back()[i]; tmp[1] = matrix[lg.back()[i]].size() - 1; nodes.push_back(tmp); } std::sort(nodes.begin(), nodes.end(), detail::cuthill_mckee_comp_func); for (std::size_t i = 0; i < nodes.size(); i++) { lg.back()[i] = nodes[i][0]; } m_min = -1; new_g = false; for (std::size_t i = 0; i < lg.back().size(); i++) { lh.clear(); detail::generate_layering(matrix, lh, lg.back()[i]); if (lh.size() > lg.size()) { g = lg.back()[i]; new_g = true; break; } m = detail::calc_layering_width(lh); if (m_min < 0 || m < m_min) { m_min = m; h = lg.back()[i]; } } } lh.clear(); detail::generate_layering(matrix, lh, h); // calculate ls as layering intersection and rg as remaining // graph lap.clear(); for (std::size_t i = 0; i < lg.size(); i++) { for (std::size_t j = 0; j < lg[i].size(); j++) { lap[lg[i][j]].resize(2); lap[lg[i][j]][0] = i; } } for (std::size_t i = 0; i < lh.size(); i++) { for (std::size_t j = 0; j < lh[i].size(); j++) { lap[lh[i][j]][1] = lg.size() - 1 - i; } } rg.clear(); ls.clear(); ls.resize(lg.size()); for (std::map< int, std::vector<int> >::iterator it = lap.begin(); it != lap.end(); it++) { if ((it->second)[0] == (it->second)[1]) { ls[(it->second)[0]].push_back(it->first); } else { rg.push_back(it->first); } } // partition remaining graph in connected components rgc = detail::gps_rg_components(matrix, n, rg); // insert nodes of each component of rgc k1 = detail::calc_layering_width(lg); k2 = detail::calc_layering_width(lh); wvs.resize(ls.size()); wvsg.resize(ls.size()); wvsh.resize(ls.size()); for (std::size_t i = 0; i < rgc.size(); i++) { for (std::size_t j = 0; j < ls.size(); j++) { wvs[j] = ls[j].size(); wvsg[j] = ls[j].size(); wvsh[j] = ls[j].size(); } for (std::size_t j = 0; j < rgc[i].size(); j++) { (wvsg[lap[rgc[i][j]][0]])++; (wvsh[lap[rgc[i][j]][1]])++; } k3 = 0; k4 = 0; for (std::size_t j = 0; j < ls.size(); j++) { if (wvsg[j] > wvs[j]) { k3 = std::max(k3, wvsg[j]); } if (wvsh[j] > wvs[j]) { k4 = std::max(k4, wvsh[j]); } } if (k3 < k4 || (k3 == k4 && k1 <= k2) ) { for (std::size_t j = 0; j < rgc[i].size(); j++) { ls[lap[rgc[i][j]][0]].push_back(rgc[i][j]); } } else { for (std::size_t j = 0; j < rgc[i].size(); j++) { ls[lap[rgc[i][j]][1]].push_back(rgc[i][j]); } } } // renumber nodes in ls rl.clear(); rl.resize(ls.size()); state = 1; state_end = false; while (!state_end) { switch(state) { case 1: l = 0; state = 4; break; case 2: for (std::size_t i = 0; i < rl[l-1].size(); i++) { isn.assign(n, false); for (std::map<int, double>::const_iterator it = matrix[rl[l-1][i]].begin(); it != matrix[rl[l-1][i]].end(); it++) { if (it->first == rl[l-1][i]) continue; isn[it->first] = true; } nodes.resize(0); for (std::size_t j = 0; j < ls[l].size(); j++) { if (inr[ls[l][j]]) continue; if (!isn[ls[l][j]]) continue; tmp[0] = ls[l][j]; tmp[1] = matrix[ls[l][j]].size() - 1; nodes.push_back(tmp); } std::sort(nodes.begin(), nodes.end(), detail::cuthill_mckee_comp_func); for (std::size_t j = 0; j < nodes.size(); j++) { rl[l].push_back(nodes[j][0]); r.push_back(nodes[j][0]); inr[nodes[j][0]] = true; } } case 3: for (std::size_t i = 0; i < rl[l].size(); i++) { isn.assign(n, false); for (std::map<int, double>::const_iterator it = matrix[rl[l][i]].begin(); it != matrix[rl[l][i]].end(); it++) { if (it->first == rl[l][i]) continue; isn[it->first] = true; } nodes.resize(0); for (std::size_t j = 0; j < ls[l].size(); j++) { if (inr[ls[l][j]]) continue; if (!isn[ls[l][j]]) continue; tmp[0] = ls[l][j]; tmp[1] = matrix[ls[l][j]].size() - 1; nodes.push_back(tmp); } std::sort(nodes.begin(), nodes.end(), detail::cuthill_mckee_comp_func); for (std::size_t j = 0; j < nodes.size(); j++) { rl[l].push_back(nodes[j][0]); r.push_back(nodes[j][0]); inr[nodes[j][0]] = true; } } case 4: if (rl[l].size() < ls[l].size()) { deg_min = -1; for (std::size_t j = 0; j < ls[l].size(); j++) { if (inr[ls[l][j]]) continue; deg = matrix[ls[l][j]].size() - 1; if (deg_min < 0 || deg < deg_min) { ind_min = ls[l][j]; deg_min = deg; } } rl[l].push_back(ind_min); r.push_back(ind_min); inr[ind_min] = true; state = 3; break; } case 5: l++; if (l < ls.size()) { state = 2; } else { state_end = true; } break; default: break; } } } return r; }