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