//-----------------------------------------------------------------------------
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;
}
