/* boilerplate to turn the queue into a real AbstractExpression tree; return the generated AE tree by reference to allow deletion (the queue is emptied by the tree building process) */ AbstractExpression * convertToExpression(queue<AE*> &e) { AE *tree = makeTree(NULL, e); json_spirit::Object json = tree->serializeValue(); AbstractExpression * exp = AbstractExpression::buildExpressionTree(json); delete tree; return exp; }
/* boilerplate to turn the queue into a real AbstractExpression tree; return the generated AE tree by reference to allow deletion (the queue is emptied by the tree building process) */ AbstractExpression * convertToExpression(queue<AE*> &e) { AE *tree = makeTree(NULL, e); Json::Value json = tree->serializeValue(); Json::FastWriter writer; std::string jsonText = writer.write(json); PlannerDomRoot domRoot(jsonText.c_str()); AbstractExpression * exp = AbstractExpression::buildExpressionTree(domRoot.rootObject()); delete tree; return exp; }
void vPreProcess::run() { Stamp zynq_stamp; Stamp local_stamp; resolution resmod = res; resmod.height -= 1; resmod.width -= 1; int nm0 = 0, nm1 = 0, nm2 = 0, nm3 = 0, nm4 = 0; AE v; SkinEvent se; SkinSample ss; bool received_half_sample = false; int32_t salvage_sample[2] = {-1, 0}; while(true) { double pyt = zynq_stamp.getTime(); std::deque<AE> qleft, qright; std::deque<int32_t> qskin; std::deque<int32_t> qskinsamples; const std::vector<int32_t> *q = inPort.read(zynq_stamp); if(!q) break; delays.push_back((Time::now() - zynq_stamp.getTime())); if(pyt) intervals.push_back(zynq_stamp.getTime() - pyt); if(precheck) { nm0 = zynq_stamp.getCount(); if(nm3 && nm0 - nm1 == 1 && nm1 - nm2 > 1 && nm1 - nm3 > 2) { yWarning() << "LOST" << nm1-nm2-1 << "PACKETS [" << nm4 << nm3 << nm2 << nm1 << nm0 << "]" << q->size() << "packet size"; } nm4 = nm3; nm3 = nm2; nm2 = nm1; nm1 = nm0; } //unsigned int events_in_packet = 0; const int32_t *qi = q->data(); while ((size_t)(qi - q->data()) < q->size()) { if(IS_SKIN(*(qi+1))) { if(IS_SAMPLE(*(qi+1))) { qskinsamples.push_back(*(qi++)); //TS qskinsamples.push_back(*(qi++)); //VALUE/TAXEL } else { qskin.push_back(*(qi++)); //TS qskin.push_back(*(qi++)); //TAXEL } } else { // IS_VISION v.decode(qi); //precheck if(precheck && (v.x < 0 || v.x > resmod.width || v.y < 0 || v.y > resmod.height)) { yWarning() << "Event Corruption:" << v.getContent().toString(); continue; } //flipx and flipy if(flipx) v.x = resmod.width - v.x; if(flipy) v.y = resmod.height - v.y; //salt and pepper filter if(pepper && !thefilter.check(v.x, v.y, v.polarity, v.channel, v.stamp)) { v_dropped++; continue; } //undistortion (including rectification) if(undistort) { cv::Vec2i mapPix; if(v.getChannel() == 0) mapPix = leftMap.at<cv::Vec2i>(v.y, v.x); else mapPix = rightMap.at<cv::Vec2i>(v.y, v.x); //truncate to sensor bounds after mapping? if(truncate && (mapPix[0] < 0 || mapPix[0] > resmod.width || mapPix[1] < 0 || mapPix[1] > resmod.height)) { continue; } v.x = mapPix[0]; v.y = mapPix[1]; //std::cout.precision(30); //std::cout<<v.channel<<mapPix<<"timestamp:"<<pyt<<std::endl; } if(split && v.channel) { qright.push_back(v); } else { qleft.push_back(v); } } } if(qskinsamples.size() > 2) { //if we have skin samples //check if we need to fix the ordering if(IS_SSV(qskinsamples[1])) { // missing address if(received_half_sample) { // but we have it from last bottle qskinsamples.push_front(salvage_sample[1]); qskinsamples.push_front(salvage_sample[0]); } else { // otherwise we are misaligned due to missing data qskinsamples.pop_front(); qskinsamples.pop_front(); } } received_half_sample = false; //either case the half sample is no longer valid //check if we now have a cut event int samples_overrun = qskinsamples.size() % packetSize(SkinSample::tag); if(samples_overrun == 2) { salvage_sample[1] = qskinsamples.back(); qskinsamples.pop_back(); salvage_sample[0] = qskinsamples.back(); qskinsamples.pop_back(); received_half_sample = true; } else if(samples_overrun) { yError() << "samples cut by " << samples_overrun; } } v_total += qleft.size() + qright.size(); if(use_local_stamp) { local_stamp.update(); zynq_stamp = local_stamp; } if(qleft.size()) { outPortCamLeft.write(qleft, zynq_stamp); } if(qright.size()) { outPortCamRight.write(qright, zynq_stamp); } if(qskin.size()) { outPortSkin.write(qskin, zynq_stamp); } if(qskinsamples.size()) { outPortSkinSamples.write(qskinsamples, zynq_stamp); } } }