int do_qvary_filters(dataptr dz)
{
int n;
int fno, chans = dz->infile->channels;
float *buf = dz->sampbuf[0];
double *ampl = dz->parray[FLT_AMPL];
double *a = dz->parray[FLT_A];
double *b = dz->parray[FLT_B];
double *y = dz->parray[FLT_Y];
double *z = dz->parray[FLT_Z];
double *d = dz->parray[FLT_D];
double *e = dz->parray[FLT_E];
for (n = 0; n < dz->ssampsread; n += chans) {
if((dz->iparam[FLT_SAMS] -= chans) <= 0) {
if(!newvalue(FLT_Q,FLT_Q_INCR,FLT_SAMS,dz)) {
sprintf(errstr,"Ran out of Q values: do_qvary_filters()\n");
return(PROGRAM_ERROR);
}
dz->iparam[FLT_SAMS] *= chans;
}
if((dz->iparam[FLT_BLOKCNT] -= chans) <= 0) {
for (fno = 0; fno < dz->iparam[FLT_CNT]; fno++)
get_coeffs2(fno,dz);
dz->param[FLT_Q] *= dz->param[FLT_Q_INCR];
dz->iparam[FLT_BLOKCNT] = dz->iparam[FLT_BLOKSIZE] * chans;
}
filtering(n,chans,buf,a,b,y,z,d,e,ampl,dz);
}
return(CONTINUE);
}
void texture::copy( const moon9::texture &that )
{
if( &that == this )
return;
#if 0
*this = that;
this->delay = that.delay;
this->u0 = that.u0;
this->v0 = that.v0;
this->u1 = that.u1;
this->v1 = that.v1;
this->delegated = true;
#else
clone( that );
this->id = 0;
this->delegated = false;
if( that.id > 0 )
{
create();
bind();
filtering(); // @todo: what about 'that' filtering settings? lost forever? :s
submit();
}
#endif
}
texture::texture( const image &i, bool mirror_w, bool mirror_h ) : std::vector<pixel>(), w(0), h(0), iw(0), ih(0), u0(0), v0(0), u1(1), v1(1), delay(0.f), id(0), delegated(false)
{
//create();
if( load( i, mirror_w, mirror_h ) )
{
bind();
filtering();
submit();
}
}
/*!
Do the filtering and prediction of the measure signal.
\param z : Measures \f${\bf z}_k\f$ used to initialise the filter. The dimension of
this vector is equal to the number of signal to filter (given by
getNumberOfSignal()) multiplied by the size of the measure vector
(given by getMeasureSize()) .
\exception vpException::notInitialized : If the filter is not
initialized. To initialize the filter see initFilter().
*/
void
vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
{
if (nsignal < 1) {
vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter") ;
throw(vpException(vpException::notInitialized,
"Bad signal number")) ;
}
// Specific initialization of the filter that depends on the state model
if (iter == 0) {
Xest = 0;
switch (model) {
case stateConstVel_MeasurePos:
case stateConstVelWithColoredNoise_MeasureVel:
case stateConstAccWithColoredNoise_MeasureVel:
for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
Xest[size_state*i] = z[i] ;
}
prediction();
// init_done = true;
break;
case unknown:
vpERROR_TRACE("Kalman state model is not set") ;
throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
break;
}
iter ++;
return;
}
else if (iter == 1) {
if (model == stateConstVel_MeasurePos) {
for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
double z_prev = Xest[size_state*i]; // Previous mesured position
// std::cout << "Mesure pre: " << z_prev << std::endl;
Xest[size_state*i] = z[i] ;
Xest[size_state*i+1] = (z[i] - z_prev) / dt ;
}
prediction();
iter ++;
return;
}
}
filtering(z);
prediction();
}
int do_filters(dataptr dz)
{
int n;
int chans = dz->infile->channels;
float *buf = dz->sampbuf[0];
double *ampl = dz->parray[FLT_AMPL];
double *a = dz->parray[FLT_A];
double *b = dz->parray[FLT_B];
double *y = dz->parray[FLT_Y];
double *z = dz->parray[FLT_Z];
double *d = dz->parray[FLT_D];
double *e = dz->parray[FLT_E];
for (n = 0; n < dz->ssampsread; n += chans)
filtering(n,chans,buf,a,b,y,z,d,e,ampl,dz);
return(CONTINUE);
}
int do_fvary2_filters(dataptr dz)
{
int exit_status;
int n, fno, chans = dz->infile->channels;
float *buf = dz->sampbuf[0];
//double *fincr = dz->parray[FLT_FINCR];
//double *aincr = dz->parray[FLT_AINCR];
double *ampl = dz->parray[FLT_AMPL];
double *a = dz->parray[FLT_A];
double *b = dz->parray[FLT_B];
double *y = dz->parray[FLT_Y];
double *z = dz->parray[FLT_Z];
double *d = dz->parray[FLT_D];
double *e = dz->parray[FLT_E];
int fsams = dz->iparam[FLT_FSAMS];
for (n = 0; n < dz->ssampsread; n += chans) {
if(dz->brksize[FLT_Q]) {
if((dz->iparam[FLT_SAMS] -= chans) <= 0) {
if(!newvalue(FLT_Q,FLT_Q_INCR,FLT_SAMS,dz)) {
sprintf(errstr,"Ran out of Q values: do_fvary2_filters()\n");
return(PROGRAM_ERROR);
}
dz->iparam[FLT_SAMS] *= chans;
}
}
if(fsams <= 0) {
if((exit_status = newfval2(dz->parray[FLT_FBRK],dz->parray[FLT_HBRK],dz))<0)
return(exit_status);
fsams = dz->iparam[FLT_FSAMS];
for (fno = 0; fno < dz->iparam[FLT_CNT]; fno++) {
get_coeffs1(fno,dz);
get_coeffs2(fno,dz);
}
if(dz->brksize[FLT_Q])
dz->param[FLT_Q] *= dz->param[FLT_Q_INCR];
}
filtering(n,chans,buf,a,b,y,z,d,e,ampl,dz);
fsams--;
}
return(CONTINUE);
}
int main(int argc, char **argv)
{
int c;
int inflag = 0;
int dbg = 0;
int sr_ds = 200;
char * rec_name = "vfdb/427";
char * db_path = "/opt/physiobank/database";
size_t win_sec = 8;
/* r stands for record with folder
* p for path
* i for information of record
* s downsample sr
* w window length default:8
* d debug
*/
while ((c = getopt(argc, argv, "idr:p:s:w:")) != -1)
switch (c){
case 'i':
inflag = 1;
break;
case 'r':
rec_name = optarg;
break;
case 'p':
db_path = optarg;
break;
case 's':
sr_ds= atoi(optarg);
break;
case'w':
win_sec = atoi(optarg);
break;
case'd':
dbg = 1;
break;
default:
abort();
}
int i, j, nsig;
WFDB_Sample *v;
WFDB_Siginfo *s;
WFDB_Anninfo a;
setwfdb(db_path);
nsig = isigopen(rec_name, NULL, 0);
if (nsig < 1){
printf("nsig:%d\n",nsig);
exit(1);
}
s = (WFDB_Siginfo *)malloc(nsig * sizeof(WFDB_Siginfo));
if (isigopen(rec_name, s, nsig) != nsig)
exit(1);
v = (WFDB_Sample *)malloc(nsig * sizeof(WFDB_Sample));
int orig_sr = sampfreq(rec_name);
a.name = "atr"; a.stat = WFDB_READ;
if (wfdbinit(rec_name, &a, 1, s, nsig) != nsig) exit(3);
if(1 == inflag ){
printf("sr:%d\n",orig_sr);
printf("%d signals\n", nsig);
for (i = 0; i < nsig; i++) {
printf("Group %d, Signal %d:\n", s[i].group, i);
printf("File: %s\n", s[i].fname);
printf("Description: %s\n", s[i].desc);
printf("Gain: ");
if (s[i].gain == 0.)
printf("uncalibrated; assume %g", WFDB_DEFGAIN);
else printf("%g", s[i].gain);
printf(" adu/%s\n", s[i].units ? s[i].units : "mV");
printf(" Initial value: %d\n", s[i].initval);
printf(" Storage format: %d\n", s[i].fmt);
printf(" I/O: ");
if (s[i].bsize == 0) printf("can be unbuffered\n");
else printf("%d-byte blocks\n", s[i].bsize);
printf(" ADC resolution: %d bits\n", s[i].adcres);
printf(" ADC zero: %d\n", s[i].adczero);
if (s[i].nsamp > 0L) {
printf(" Length: %s (%ld sample intervals)\n",
timstr(s[i].nsamp), s[i].nsamp);
printf(" Checksum: %d\n", s[i].cksum);
}
else printf(" Length undefined\n");
}
}
fifo_t fifo_ecg;
int fifo_ecg_buf[FIFO_SIZE];
fifo_init(&fifo_ecg, fifo_ecg_buf, FIFO_SIZE);
fifo_t fifo_bt;
int fifo_bt_buf[FIFO_SIZE];
fifo_init(&fifo_bt, fifo_bt_buf, FIFO_SIZE);
int tmp = 0;
int sr = 200;
WFDB_Time begin_samp = 0;
WFDB_Time end_samp = orig_sr*win_sec;
WFDB_Annotation begin_ann;
WFDB_Annotation end_ann;
getann(0, &begin_ann);
while(0 == getann(0, &end_ann))
if ((end_ann.aux != NULL && *end_ann.aux > 0)
||0 == strcmp(annstr(end_ann.anntyp), "[")
||0 == strcmp(annstr(end_ann.anntyp), "]")
||0 == strcmp(annstr(end_ann.anntyp), "~")
){
break;
}
int * pBt_len = (int*)calloc(win_sec,sizeof(int));
ResetBDAC();
for (; ;) {
if (getvec(v) < 0)
break;
// for (j = 0; j < nsig; j++){
// }
tmp = v[nsig-1];
tmp = v[0];
int vout1 = 0;
static int bt_i = 0;
static unsigned int samplecnt = 0;
int idx = 0;
if(down_sample(tmp, &vout1, orig_sr, sr)) {
samplecnt ++;
fifo_write(&fifo_ecg, &vout1, 1*sizeof(int));
int beatType, beatMatch;
long ltmp = vout1-s[0].adczero;
ltmp *= 200; ltmp /= s[0].gain;
int bdac_dly = BeatDetectAndClassify(ltmp, &beatType, &beatMatch);
idx = bt_i/sr;
if (0 != bdac_dly ) {
pBt_len[idx]++;
fifo_write(&fifo_bt, &beatType, sizeof(int));
}
bt_i = ++bt_i%(win_sec*sr);
}
double cm = 0.0;
int size = win_sec*sr;
if(fifo_len(&fifo_ecg)/sizeof(int) >= sr*win_sec){
int * win_data = (int*)malloc(win_sec*sr*sizeof(int));
int * ds_data = (int*)malloc(win_sec*sr_ds*sizeof(int));
int len = fifo_len(&fifo_bt)/sizeof(int);
int * p = (int*)calloc(len, sizeof(int));
if (0 != len){
//printf("bt_i:%d\n", bt_i);
fifo_read_steps(&fifo_bt, p, len*sizeof(int), pBt_len[bt_i/sr]*sizeof(int));
//for (i = 0;i< len;i++) printf("%d ", p[i]);
//printf("\n");
}
pBt_len[bt_i/sr] = 0;
fifo_read_steps(&fifo_ecg, win_data, size*sizeof(int), sr*sizeof(int));
filtering(win_data, size, sr);
int i = 0;
int ds_size = 0;
int vout;
for(i = 0;i < size;i++){
// if(down_sample(win_data[i], &vout, sr, sr_ds)) ds_data[ds_size++] = vout;
}
double dven = 0.0;
if (0 != len){
int tmp_cnt = 0;
for(i = 0; i < len ; i++){
if(5 == p[i]) dven ++;
}
//printf("div:%d %d\n", tmp_cnt, len);
dven /= len;
}
//cm = ecg_complexity_measure(win_data, size);
//cm = ecg_complexity_measure(ds_data, ds_size);
//cm = cpsd(ds_data, ds_size, 0.5*sr_ds);
//cm = calc_grid(ds_data, ds_size, 0.5*sr_ds);
cm = calc_grid(win_data, size, 0.5*sr);
if (-1 == cm ) continue;
//VT print 1;
//VF print 2;
int hr = (int)((double)(len*60)/win_sec+0.5);
int ret = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(VT");
if (1 == ret){
printf("%d %lf %lf %d\n", 1, cm, dven, hr);
} else if (0 == ret) {
ret = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(VFL");
int ret2 = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "[");
if (1 == ret || 1 == ret2){
printf("%d %lf %lf %d\n", 2, cm, dven, hr);
} else if (0 == ret){
// ret = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(N");
// int ret3 = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "N");
// if ((1 == ret || 1 == ret3) && -1 != begin_ann.subtyp) printf ("%d %lf %lf %d\n", 0, cm, dven, hr);
if (1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(AFIB")
|| 1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(AFL")
|| 1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(IVR")
|| 1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(SVTA")
|| 1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(SBR")
|| 1 == check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(BII")
) printf ("%d %lf %lf %d\n", 0, cm, dven, hr);
// int ret = check_ann(begin_samp, end_samp, &begin_ann, &end_ann, "(SVTA");
// if (1 == ret) printf ("%d %lf\n", 0, cm);
}
}
//iannsettime(begin_samp);
begin_samp += orig_sr;
end_samp += orig_sr;
if(end_ann.time < begin_samp){
begin_ann = end_ann;
while(1)
if (0 != getann(0, &end_ann)){
end_ann = begin_ann;
/*the last sample of the signal*/
end_ann.time = s[nsig-1].nsamp;
break;
}else if ((end_ann.aux != NULL && *end_ann.aux > 0)
||0 == strcmp(annstr(end_ann.anntyp), "[")
||0 == strcmp(annstr(end_ann.anntyp), "]")
||0 == strcmp(annstr(end_ann.anntyp), "~")
){
break;
}
if(dbg){
printf("begin tm:%s type:%s ", mstimstr(begin_ann.time), annstr(begin_ann.anntyp));
if(begin_ann.aux != NULL)
printf("begin aux:%s", begin_ann.aux+1);
printf("\n");
printf("end tm:%s type:%s ", mstimstr(end_ann.time), annstr(end_ann.anntyp));
if (end_ann.aux != NULL)
printf("end aux:%s", end_ann.aux+1);
printf("\n");
printf("begin sample:%s\n", mstimstr(begin_samp));
printf("ann diff:%s\n", mstimstr(end_ann.time-begin_ann.time));
}
}
free(p);
free(ds_data);
free(win_data);
}
}
free(pBt_len);
wfdbquit();
return 0;
}
// Use boost::program_options to parse command line input
// This does pretty much all the parameter checking needed
int parseCommandLine (int argc, char *argv[], po::variables_map& vm)
{
int failed = 0;
//Required options
po::options_description required("Required options");
required.add_options()
("kmers,k", po::value<std::string>(), "dsm kmer output file (not needed if using --mds_concat)")
("pheno,p", po::value<std::string>(), ".pheno metadata");
po::options_description mds("MDS options");
mds.add_options()
("output,o", po::value<std::string>(), "output prefix for new dsm file")
("no_mds", "do not perform MDS; output subsampled matrix instead")
("write_distances", "write csv of distance matrix")
("mds_concat", po::value<std::string>(), "list of subsampled matrices to use in MDS. Performs only MDS; implies --no_filtering")
("pc", po::value<int>()->default_value(pc_default), "number of principal coordinates to output")
("size", po::value<long int>()->default_value(size_default), "number of kmers to use in MDS")
("threads", po::value<int>()->default_value(1), ("number of threads. Suggested: " + std::to_string(std::thread::hardware_concurrency())).c_str());
//Optional filtering parameters
//NB pval cutoffs are strings for display, and are converted to floats later
po::options_description filtering("Filtering options");
filtering.add_options()
("no_filtering", "turn off all filtering and do not output new kmer file")
("max_length", po::value<long int>()->default_value(max_length_default), "maximum kmer length")
("maf", po::value<double>()->default_value(maf_default), "minimum kmer frequency")
("min_words", po::value<int>(), "minimum kmer occurences. Overrides --maf");
po::options_description other("Other options");
other.add_options()
("version", "prints version and exits")
("help,h", "full help message");
po::options_description all;
all.add(required).add(mds).add(filtering).add(other);
try
{
po::store(po::command_line_parser(argc, argv).options(all).run(), vm);
if (vm.count("help"))
{
printHelp(all);
failed = 1;
}
else if (vm.count("version"))
{
std::cout << VERSION << std::endl;
failed = 1;
}
else
{
po::notify(vm);
failed = 0;
// Check input files exist, and can stat
if ((vm.count("kmers") && !fileStat(vm["kmers"].as<std::string>())) || (vm.count("pheno") && !fileStat(vm["pheno"].as<std::string>())))
{
failed = 1;
}
}
}
catch (po::error& e)
{
// Report errors from boost library
std::cerr << "Error in command line input: " << e.what() << "\n";
std::cerr << "Run 'kmds --help' for full option listing\n\n";
std::cerr << required << "\n" << other << "\n";
failed = 1;
}
return failed;
}
bool event_grapht::graph_explorert::backtrack(
std::set<critical_cyclet> &set_of_cycles,
unsigned source,
unsigned vertex,
bool unsafe_met, /* unsafe pair for the model met in the visited path */
unsigned po_trans, /* po-transition skips still allowed */
bool same_var_pair, /* in a thread, tells if we already met one rfi wsi fri */
bool lwfence_met, /* if we try to skip a lwsync (only valid for lwsyncWR) */
bool has_to_be_unsafe,
irep_idt var_to_avoid,
memory_modelt model)
{
#ifdef DEBUG
for(unsigned i=0; i<80; egraph.message.debug() << "-", ++i);
egraph.message.debug() << messaget::eom;
egraph.message.debug() << "marked size:" << marked_stack.size()
<< messaget::eom;
std::stack<unsigned> tmp;
while(!point_stack.empty())
{
egraph.message.debug() << point_stack.top() << " | ";
tmp.push(point_stack.top());
point_stack.pop();
}
egraph.message.debug() << messaget::eom;
while(!tmp.empty())
{
point_stack.push(tmp.top());
tmp.pop();
}
while(!marked_stack.empty())
{
egraph.message.debug() << marked_stack.top() << " | ";
tmp.push(marked_stack.top());
marked_stack.pop();
}
egraph.message.debug() << messaget::eom;
while(!tmp.empty())
{
marked_stack.push(tmp.top());
tmp.pop();
}
#endif
// TO DISCUSS: shouldn't we still allow po-transition through it instead?
if(filtering(vertex))
return false;
egraph.message.debug() << "bcktck "<<egraph[vertex].id<<"#"<<vertex<<", "
<<egraph[source].id<<"#"<<source<<" lw:"<<lwfence_met<<" unsafe:"
<<unsafe_met << messaget::eom;
bool f=false;
bool get_com_only=false;
bool unsafe_met_updated=unsafe_met;
bool same_var_pair_updated=same_var_pair;
bool not_thin_air=true;
const abstract_eventt& this_vertex=egraph[vertex];
/* if a thread starts with variable x, the last event of this thread in the
cycle cannot be with x */
irep_idt avoid_at_the_end=var_to_avoid;
bool no_comm=false;
if(avoid_at_the_end!="" && avoid_at_the_end==this_vertex.variable)
no_comm=true;
/* if specified, maximum number of events reached */
if(max_var!=0 && point_stack.size()>max_var*3)
return false;
/* we only explore shared variables */
if(!this_vertex.local)
{
/* only the lwsyncWR can be interpreted as poWR (i.e., skip of the fence) */
if(lwfence_met && this_vertex.operation!=abstract_eventt::Read)
return false; //{no_comm=true;get_com_only=false;}//return false;
bool has_to_be_unsafe_updated=false;
// TODO: propagate this constraint within the optimisation
// -- no optimisation can strongly affect performances
/* tab[] can appear several times */
if(egraph.ignore_arrays || id2string(this_vertex.variable).find("[]")==std::string::npos)
{
/* no more than 4 events per thread */
if(this_vertex.operation!=abstract_eventt::Fence
&& this_vertex.operation!=abstract_eventt::Lwfence
&& this_vertex.operation!=abstract_eventt::ASMfence)
{
if(events_per_thread[this_vertex.thread]==4)
return false;
else
events_per_thread[this_vertex.thread]++;
}
/* Multiple re-orderings constraint: if the thread on this cycles contains
more than one, ensure that an unsafe pair is not protected by another
relation in the thread. E.g., in Wx Rx Wy, under TSO, the rfi cannot be
delayed, since the only way to make this transformation matter is to
re-order also the two writes, which is not permitted on TSO. */
if(has_to_be_unsafe && point_stack.size() >= 2)
{
const unsigned previous = point_stack.top();
point_stack.pop();
const unsigned preprevious = point_stack.top();
point_stack.push(previous);
if(!egraph[preprevious].unsafe_pair(this_vertex,model)
&& !(this_vertex.operation==abstract_eventt::Fence
|| egraph[preprevious].operation==abstract_eventt::Fence
|| this_vertex.operation==abstract_eventt::Lwfence
|| egraph[preprevious].operation==abstract_eventt::Lwfence
|| this_vertex.operation==abstract_eventt::ASMfence
|| egraph[preprevious].operation==abstract_eventt::ASMfence))
return false;
}
}
has_to_be_unsafe_updated = has_to_be_unsafe;
/* constraint 1.a: there is at most one pair of events per thread
with different variables. Given that we cannot have more than
three events on the same variable, with two in the same thread,
this means that we can have at most 2 consecutive events by po
with the same variable, and two variables per thread (fences are
not taken into account) */
if(!point_stack.empty() && egraph.are_po_ordered(point_stack.top(),vertex)
&& this_vertex.operation!=abstract_eventt::Fence
&& this_vertex.operation!=abstract_eventt::Lwfence
&& this_vertex.operation!=abstract_eventt::ASMfence
&& this_vertex.variable==egraph[point_stack.top()].variable)
{
if(same_var_pair ||
(this_vertex.operation==abstract_eventt::Read
&& egraph[point_stack.top()].operation==abstract_eventt::Read))
{
events_per_thread[this_vertex.thread]--;
return false; //{no_comm=true;get_com_only=false;} //return false;
}
else
{
same_var_pair_updated = true;
if(events_per_thread[this_vertex.thread]>=3)
get_com_only = true;
}
}
/* constraint 1.b */
if(!point_stack.empty() && egraph.are_po_ordered(point_stack.top(),vertex)
&& this_vertex.operation!=abstract_eventt::Fence
&& this_vertex.operation!=abstract_eventt::Lwfence
&& this_vertex.operation!=abstract_eventt::ASMfence
&& this_vertex.variable!=egraph[point_stack.top()].variable)
{
same_var_pair_updated = false;
}
/* constraint 2: per variable, either W W, R W, W R, or R W R */
if(this_vertex.operation!=abstract_eventt::Fence
&& this_vertex.operation!=abstract_eventt::Lwfence
&& this_vertex.operation!=abstract_eventt::ASMfence)
{
const unsigned char nb_writes = writes_per_variable[this_vertex.variable];
const unsigned char nb_reads = reads_per_variable[this_vertex.variable];
if(nb_writes+nb_reads==3)
{
events_per_thread[this_vertex.thread]--;
return false; //{no_comm=true;get_com_only=false;} //return false;
}
else if(this_vertex.operation==abstract_eventt::Write)
{
if(nb_writes==2)
{
events_per_thread[this_vertex.thread]--;
return false; //{no_comm=true;get_com_only=false;} //return false;
}
else
writes_per_variable[this_vertex.variable]++;
}
else if(this_vertex.operation==abstract_eventt::Read)
{
if(nb_reads==2)
{
events_per_thread[this_vertex.thread]--;
return false; //{no_comm=true;get_com_only=false;} //return false;
}
else
reads_per_variable[this_vertex.variable]++;
}
}
if(!point_stack.empty())
{
const abstract_eventt& prev_vertex = egraph[point_stack.top()];
unsafe_met_updated |= (prev_vertex.unsafe_pair(this_vertex,model)
&& !(prev_vertex.thread==this_vertex.thread
&& egraph.map_data_dp[this_vertex.thread].dp(prev_vertex,this_vertex)));
if (unsafe_met_updated && !unsafe_met
&& egraph.are_po_ordered(point_stack.top(), vertex))
has_to_be_unsafe_updated=true;
}
point_stack.push(vertex);
mark[vertex]=true;
marked_stack.push(vertex);
if(!get_com_only)
{
/* we first visit via po transition, if existing */
for(graph<abstract_eventt>::edgest::const_iterator
w_it=egraph.po_out(vertex).begin();
w_it!=egraph.po_out(vertex).end(); w_it++)
{
const unsigned w = w_it->first;
if(w == source && point_stack.size()>=4
&& (unsafe_met_updated
|| this_vertex.unsafe_pair(egraph[source],model)) )
{
critical_cyclet new_cycle = extract_cycle(vertex, source, cycle_nb++);
not_thin_air = !egraph.filter_thin_air || new_cycle.is_not_thin_air();
if(!not_thin_air)
{
for(critical_cyclet::const_iterator e_it=new_cycle.begin();
e_it!=new_cycle.end();
++e_it)
thin_air_events.insert(*e_it);
}
if((!egraph.filter_uniproc || new_cycle.is_not_uniproc(model)) && not_thin_air
&& new_cycle.is_cycle() &&
new_cycle.is_unsafe(model) /*&&
new_cycle.is_unsafe_asm(model)*/)
{
egraph.message.debug() << new_cycle.print_name(model,false)
<< messaget::eom;
set_of_cycles.insert(new_cycle);
#if 0
const critical_cyclet* reduced=new_cycle.hide_internals();
set_of_cycles.insert(*reduced);
delete(reduced);
#endif
}
f = true;
}
else if(!mark[w])
f |= backtrack(set_of_cycles, source, w, unsafe_met_updated,
po_trans, same_var_pair_updated, false, has_to_be_unsafe_updated,
avoid_at_the_end, model);
}
}
if(!no_comm)
/* we then visit via com transitions, if existing */
for(graph<abstract_eventt>::edgest::const_iterator
w_it=egraph.com_out(vertex).begin();
w_it!=egraph.com_out(vertex).end(); w_it++)
{
const unsigned w = w_it->first;
if(w < source)
egraph.remove_com_edge(vertex,w);
else if(w == source && point_stack.size()>=4
&& (unsafe_met_updated
|| this_vertex.unsafe_pair(egraph[source],model)) )
{
critical_cyclet new_cycle = extract_cycle(vertex, source, cycle_nb++);
not_thin_air = !egraph.filter_thin_air || new_cycle.is_not_thin_air();
if(!not_thin_air)
{
for(critical_cyclet::const_iterator e_it=new_cycle.begin();
e_it!=new_cycle.end();
++e_it)
thin_air_events.insert(*e_it);
}
if((!egraph.filter_uniproc || new_cycle.is_not_uniproc(model)) && not_thin_air
&& new_cycle.is_cycle() &&
new_cycle.is_unsafe(model) /*&&
new_cycle.is_unsafe_asm(model)*/)
{
egraph.message.debug() << new_cycle.print_name(model,false)
<< messaget::eom;
set_of_cycles.insert(new_cycle);
#if 0
const critical_cyclet* reduced=new_cycle.hide_internals();
set_of_cycles.insert(*reduced);
delete(reduced);
#endif
}
f = true;
}
else if(!mark[w])
f |= backtrack(set_of_cycles, source, w,
unsafe_met_updated, po_trans, false, false, false, "", model);
}
if(f)
{
while(!marked_stack.empty() && marked_stack.top()!=vertex)
{
unsigned up = marked_stack.top();
marked_stack.pop();
mark[up] = false;
}
if(!marked_stack.empty())
marked_stack.pop();
mark[vertex] = false;
}
assert(!point_stack.empty());
point_stack.pop();
/* removes variable access */
if(this_vertex.operation!=abstract_eventt::Fence
&& this_vertex.operation!=abstract_eventt::Lwfence
&& this_vertex.operation!=abstract_eventt::ASMfence)
{
if(this_vertex.operation==abstract_eventt::Write)
writes_per_variable[this_vertex.variable]--;
else
reads_per_variable[this_vertex.variable]--;
events_per_thread[this_vertex.thread]--;
}
}
/* transitivity of po: do the same, but skip this event
(except if it is a fence or no more po-transition skips allowed);
if the cycle explored so far has a thin-air subcycle, this cycle
is not valid: stop this exploration here */
if( skip_tracked.find(vertex)==skip_tracked.end() ) // 25 oct
if( not_thin_air
&& !get_com_only && (po_trans > 1 || po_trans==0)
&& !point_stack.empty() && egraph.are_po_ordered(point_stack.top(),vertex)
&& this_vertex.operation!=abstract_eventt::Fence
&& ( this_vertex.operation!=abstract_eventt::Lwfence
|| egraph[point_stack.top()].operation==abstract_eventt::Write)
&& ( this_vertex.operation!=abstract_eventt::ASMfence
|| !this_vertex.WRfence
|| egraph[point_stack.top()].operation==abstract_eventt::Write)
)
{
skip_tracked.insert(vertex);
std::stack<unsigned> tmp;
while(marked_stack.size()>0 && marked_stack.top()!=vertex)
{
tmp.push(marked_stack.top());
mark[marked_stack.top()]=false;
marked_stack.pop();
}
if(marked_stack.size()>0)
{
assert(marked_stack.top()==vertex);
mark[vertex]=true;
}
else
{
while(tmp.size()>0)
{
marked_stack.push(tmp.top());
mark[tmp.top()]=true;
tmp.pop();
}
mark[vertex]=true;
marked_stack.push(vertex);
}
if(!egraph[point_stack.top()].unsafe_pair(this_vertex, model))
{
/* tab[] should never be avoided */
if(egraph.ignore_arrays
|| id2string(this_vertex.variable).find("[]")==std::string::npos)
avoid_at_the_end = this_vertex.variable;
}
/* skip lwfence by po-transition only if we consider a WR */
// TO CHECK
const bool is_lwfence = (this_vertex.operation==abstract_eventt::Lwfence
&& egraph[point_stack.top()].operation==abstract_eventt::Write)
|| (this_vertex.operation==abstract_eventt::ASMfence &&
(!this_vertex.WRfence
&& egraph[point_stack.top()].operation==abstract_eventt::Write));
for(graph<abstract_eventt>::edgest::const_iterator w_it=
egraph.po_out(vertex).begin();
w_it!=egraph.po_out(vertex).end(); w_it++)
{
const unsigned w = w_it->first;
f |= backtrack(set_of_cycles, source, w,
unsafe_met/*_updated*/, (po_trans==0?0:po_trans-1),
same_var_pair/*_updated*/, is_lwfence, has_to_be_unsafe, avoid_at_the_end,
model);
}
if(f)
{
while(!marked_stack.empty() && marked_stack.top()!=vertex)
{
unsigned up = marked_stack.top();
marked_stack.pop();
mark[up] = false;
}
if(!marked_stack.empty())
marked_stack.pop();
mark[vertex] = false;
}
skip_tracked.erase(vertex);
}
return f;
}
bool change_detection (tms_msg_ss::ods_furniture::Request &req, tms_msg_ss::ods_furniture::Response &res)
{
//***************************
// local variable declaration
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << mkdir("/tmp/tms_ss_ods", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection/table", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
//***************************
// capture kinect data
//***************************
tms_msg_ss::ods_pcd srv;
srv.request.id = 3;
if(commander_to_kinect_capture.call(srv)){
pcl::fromROSMsg (srv.response.cloud, *cloud1);
}
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
//pcl::fromROSMsg (req.model, *model);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
TABLE = req.id;
if(TABLE == 1)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table1.pcd", *model);
else if(TABLE == 2)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table2.pcd", *model);
//***************************
// Fill in the cloud data
//***************************
// table.x = req.furniture.position.x/1000.0;
// table.y = req.furniture.position.y/1000.0;
// table.z = req.furniture.position.z/1000.0;
// table.theta = req.furniture.orientation.z;
// robot.x = req.robot.x/1000.0;
// robot.y = req.robot.y/1000.0;
// robot.theta = req.robot.theta;
// sensor.x = req.sensor.position.x;
// sensor.y = req.sensor.position.y;
// sensor.z = req.sensor.position.z;
// sensor.theta = req.sensor.orientation.y;
table.x = 1.0;
table.y = 1.5;
table.z = 0.7;
table.theta = 0.0;
robot.x = 2.6;
robot.y = 1.9;
robot.theta = 180.0;
sensor.x = 0.0;
sensor.y = 0.0;
sensor.z = 1.0;
sensor.theta = 20.0;
std::cout << robot.x << " " << robot.y << " " << robot.theta << std::endl;
//***************************
// transform input cloud
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tfm_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
transformation(*cloud1, *model);
for(int i=0;i<model->points.size();i++){
model->points[i].r = 255;
model->points[i].g = 0;
model->points[i].b = 0;
}
*tfm_cloud = *model + *cloud1;
if(!(tfm_cloud->points.size())){
std::cout << "tfm_cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/tfm_cloud1.pcd", *tfm_cloud);
//***************************
// filtering by using model
//***************************
std::cout << "filtering" << std::endl;
filtering(*cloud1, *model);
if(!(cloud1->points.size())){
std::cout << "filtered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/filter.pcd", *cloud1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dsp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud1, *dsp_cloud);
downsampling(*dsp_cloud, 0.01);
//***************************
// registration between two input pcd data
//***************************
std::cout << "registration" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgs_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr view_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Matrix4f m;
int n = 0;
while (1){
registration(*dsp_cloud, *model, *rgs_cloud, *cloud1, m);
if((double)(m(0,0)+m(1,1)+m(2,2)+m(3,3)) >= 4){
if(n > 2) break;
}
n++;
}
pcl::copyPointCloud(*cloud1, *view_cloud);
if(!(rgs_cloud->points.size())){
std::cout << "registered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/rgs_cloud.pcd", *rgs_cloud);
//***************************
// init t_voxel
//***************************
std::cout << "init table_voxel" << std::endl;
make_tablevoxel(*model);
//***************************
// remove table
//***************************
std::cout << "remove table" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud2);
remove_table(*cloud1, *rmc_cloud1);
if(!(rmc_cloud1->size() != 0)){
std::cout << "removed table cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/remove_table1.pcd", *rmc_cloud1, false);
//***************************
// segmentation
//***************************
segmentation(*rmc_cloud1, 0.015, 1);
if(rmc_cloud2->size() != 0)
segmentation(*rmc_cloud2, 0.015, 2);
if(rmc_cloud1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud1, true);
if(rmc_cloud2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/obj_cloud2.pcd", *rmc_cloud2, true);
//***************************
// compare segments with memory
//***************************
segment_matching();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb2 (new pcl::PointCloud<pcl::PointXYZRGB>);
//***************************************
// rewrite Object data on TMS database
//***************************************
tms_msg_db::tmsdb_ods_object_data srv3;
ros::Time time = ros::Time::now() + ros::Duration(9*60*60);
int c=0;
float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};
for(int i=0;i<Object_Map.size();i++){
std::cout << i << " → " << Object_Map[i].tf << std::endl;
if((Object_Map[i].tf == 1)||(Object_Map[i].tf == 2)){
std::stringstream ss;
ss << "/tmp/tms_ss_ods/ods_change_detection/table/result_rgb" << i << ".pcd";
if(Object_Map[i].cloud_rgb.size() != 0){
pcl::io::savePCDFile(ss.str (), Object_Map[i].cloud_rgb);
for(int j=0;j<Object_Map[i].cloud_rgb.points.size();j++){
Object_Map[i].cloud_rgb.points[j].r = colors[c%6][0];
Object_Map[i].cloud_rgb.points[j].g = colors[c%6][1];
Object_Map[i].cloud_rgb.points[j].b = colors[c%6][2];
}
c++;
}
else
std::cout << "no cloud_rgb data" << std::endl;
if(Object_Map[i].tf == 1){
*tmp_rgb1 += Object_Map[i].cloud_rgb;
tms_msg_db::tmsdb_data data;
data.tMeasuredTime = time;
data.iType = 5;
data.iID = 53;
data.fX = 1000.0 * (Object_Map[i].g.x - 1.0);
data.fY = 1000.0 * (Object_Map[i].g.y - 1.5);
data.fZ = 700.0;
data.fTheta = 0.0;
data.iPlace = 14;
data.iState = 1;
srv3.request.srvTMSInfo.push_back(data);
geometry_msgs::Pose pose;
pose.position.x = Object_Map[i].g.x; pose.position.y = Object_Map[i].g.y; pose.position.z = Object_Map[i].g.z;
pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 0.0;
res.objects.poses.push_back(pose);
std::cout << Object_Map[i].g.x << " " << Object_Map[i].g.y << " " << Object_Map[i].g.z << std::endl;
}
else if(Object_Map[i].tf == 2)
*tmp_rgb2 += Object_Map[i].cloud_rgb;
}
}
if(srv3.request.srvTMSInfo.size() != 0){
if(commander_to_ods_object_data.call(srv3))
std::cout << "Rewrite TMS database!!" << std::endl;
}
if((tmp_rgb1->size() == 0) && (tmp_rgb2->size() == 0)){
std::cout << "No change on table!!" << std::endl;
return true;
}
if(tmp_rgb1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result1.pcd", *tmp_rgb1, true);
if(tmp_rgb2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result2.pcd", *tmp_rgb2, true);
*view_cloud += *tmp_rgb1;
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/view_result.pcd", *view_cloud, true);
//pcl::toROSMsg (*cloud1, res.cloud);
Object_Map.clear();
return true;
}
int main(int argc, char** argv) {
float** testRedChannel = generateImageArray("red.txt");
float** testGreenChannel = generateImageArray("green.txt");
float** testBlueChannel = generateImageArray("blue.txt");
int h = getImageWidth("red.txt");
int w = getImageHeight("red.txt");
printf("%i rows x %i columns\n", h, w);
///////////////////////////////////////////////////////////
//////Compute the l1-norm distance of neighbor pixels./////
///////////////////////////////////////////////////////////
float** dIcdxRed = diffH(testRedChannel, h,w);
float** dIcdxGreen = diffH(testGreenChannel, h,w);
float** dIcdxBlue = diffH(testBlueChannel, h,w);
float** dIcdx = addChannels(dIcdxRed, dIcdxGreen, dIcdxBlue, h,w);
destroyArray(dIcdxRed);
destroyArray(dIcdxGreen);
destroyArray(dIcdxBlue);
float** dIcdyRed = diffV(testRedChannel, h,w);
float** dIcdyGreen = diffV(testGreenChannel, h,w);
float** dIcdyBlue = diffV(testBlueChannel, h,w);
float** dIcdy = addChannels(dIcdyRed, dIcdyGreen, dIcdyBlue, h,w);
destroyArray(dIcdyRed);
destroyArray(dIcdyGreen);
destroyArray(dIcdyBlue);
///////////////////////////////////////////////////////////
///////Compute the derivatives of the horizontal///////////
///////and vertical domain transforms./////////////////////
///////////////////////////////////////////////////////////
float factor = sigma_s / sigma_r;
for (int i = 0; i < h; i++) {
for (int j = 0; j < w; j++) {
dIcdx[i][j] = 1 + factor * dIcdx[i][j];
}
}
for (int i = 0; i < h; i++) {
for (int j = 0; j < w; j++) {
dIcdy[i][j] = 1 + factor * dIcdy[i][j];
}
}
///The vertical pass is performed using a transposed image.
float** dHdyT = transpose(dIcdy, h, w);
destroyArray(dIcdy);
///////////////////////////////////////////////////////////
///////////////////Perform the filtering.//////////////////
///////////////////////////////////////////////////////////
filtering(testRedChannel, dIcdx, dHdyT, h, w, 10);
filtering(testGreenChannel, dIcdx, dHdyT, h, w, 10);
filtering(testBlueChannel, dIcdx, dHdyT, h, w, 10);
///////////////////////////////////////////////////////////
///////////////////GENERATE OUTPUT IMAGE///////////////////
///////////////////////////////////////////////////////////
generateImageFile(testRedChannel, "outred.txt", h, w);
generateImageFile(testGreenChannel, "outgreen.txt", h, w);
generateImageFile(testBlueChannel, "outblue.txt", h, w);
///////////////////////////////////////////////////////////
///////////////////////FREE MEMORY/////////////////////////
///////////////////////////////////////////////////////////
destroyArray(dIcdx);
destroyArray(dHdyT);
destroyArray(testRedChannel);
destroyArray(testGreenChannel);
destroyArray(testBlueChannel);
return 0;
}
int do_fvary_filters(dataptr dz)
{
int exit_status;
int n, m, fno, chans = dz->infile->channels;
float *buf = dz->sampbuf[0];
double *fincr = dz->parray[FLT_FINCR];
double *aincr = dz->parray[FLT_AINCR];
double *ampl = dz->parray[FLT_AMPL];
double *a = dz->parray[FLT_A];
double *b = dz->parray[FLT_B];
double *y = dz->parray[FLT_Y];
double *z = dz->parray[FLT_Z];
double *d = dz->parray[FLT_D];
double *e = dz->parray[FLT_E];
int fsams = dz->iparam[FLT_FSAMS];
if (dz->vflag[DROP_OUT_AT_OVFLOW]) {
for (n = 0; n < dz->ssampsread; n += chans) {
if(fsams <= 0) {
if((exit_status = newfval(&fsams,dz))<0)
return(exit_status);
}
if(dz->brksize[FLT_Q]) {
if((dz->iparam[FLT_SAMS] -= chans) <= 0) {
if(!newvalue(FLT_Q,FLT_Q_INCR,FLT_SAMS,dz)) {
sprintf(errstr,"Ran out of Q values: do_fvary_filters()\n");
return(PROGRAM_ERROR);
}
dz->iparam[FLT_SAMS] *= chans;
}
}
if((dz->iparam[FLT_BLOKCNT] -= chans) <= 0) {
for (fno = 0; fno < dz->iparam[FLT_CNT]; fno++) {
get_coeffs1(fno,dz);
get_coeffs2(fno,dz);
}
if(dz->brksize[FLT_Q])
dz->param[FLT_Q] *= dz->param[FLT_Q_INCR];
for(m=0;m<dz->iparam[FLT_CNT];m++) {
dz->parray[FLT_FRQ][m] *= fincr[m];
dz->parray[FLT_AMP][m] *= aincr[m];
}
dz->iparam[FLT_BLOKCNT] = dz->iparam[FLT_BLOKSIZE] * chans;
}
filtering(n,chans,buf,a,b,y,z,d,e,ampl,dz);
if(dz->iparam[FLT_OVFLW] > 0) {
sprintf(errstr,"Filter overflowed\n");
return(GOAL_FAILED);
}
fsams--;
}
} else {
for (n = 0; n < dz->ssampsread; n += chans) {
if(fsams <= 0) {
if((exit_status = newfval(&fsams,dz))<0)
return(exit_status);
}
if(dz->brksize[FLT_Q]) {
if((dz->iparam[FLT_SAMS] -= chans) <= 0) {
if(!newvalue(FLT_Q,FLT_Q_INCR,FLT_SAMS,dz)) {
sprintf(errstr,"Ran out of Q values: do_fvary_filters()\n");
return(PROGRAM_ERROR);
}
dz->iparam[FLT_SAMS] *= chans;
}
}
if((dz->iparam[FLT_BLOKCNT] -= chans) <= 0) {
for (fno = 0; fno < dz->iparam[FLT_CNT]; fno++) {
get_coeffs1(fno,dz);
get_coeffs2(fno,dz);
}
if(dz->brksize[FLT_Q])
dz->param[FLT_Q] *= dz->param[FLT_Q_INCR];
for(m=0;m<dz->iparam[FLT_CNT];m++) {
dz->parray[FLT_FRQ][m] *= fincr[m];
dz->parray[FLT_AMP][m] *= aincr[m];
}
dz->iparam[FLT_BLOKCNT] = dz->iparam[FLT_BLOKSIZE] * chans;
}
filtering(n,chans,buf,a,b,y,z,d,e,ampl,dz);
fsams--;
}
}
dz->iparam[FLT_FSAMS] = fsams;
return(CONTINUE);
}
