void operator()(R &r, boost::uint8_t value) { switch(this->state) { case state_t::none: switch(value) { case dle: this->state = state_t::dle; break; default: r(data_t(value)); break; } break; case state_t::dle: this->state = state_t::none; switch(value) { case dle: r(data_t(dle)); break; case etx: r(end_t()); break; default: r(id_t(value)); break; } break; } }
static gboolean init_db(sqlite3 *db, GError **err) { int ret; if (!begin_t(db, err)) return FALSE; ret = sqlite3_exec(db, SCHEMA, NULL, NULL, NULL); if (ret != SQLITE_OK) { report_sqlite_error(db, ret, err); return FALSE; } ret = sqlite3_exec(db, "INSERT INTO meta (schemaver) " "VALUES (" SCHEMA_VERSION_STRING ")", NULL, NULL, NULL); if (ret != SQLITE_OK) { report_sqlite_error(db, ret, err); return FALSE; } if (!end_t(db, err)) return FALSE; return TRUE; }
gboolean journal_store_put_group(JournalStore *js, LJEntry **entries, int c, GError **err) { int i; if (!begin_t(js->db, err)) goto err; for (i = 0; i < c; i++) { if (!journal_store_put(js, entries[i], err)) goto err; } if (!end_t(js->db, err)) goto err; return TRUE; err: end_t(js->db, NULL); return FALSE; }
gboolean journal_store_end_group(JournalStore *js, GError **err) { return end_t(js->db, err); }
bool DynamicTransformationElement::getTransformation(const base::Time& atTime, bool doInterpolation, transformer::TransformationType& result) { if(!gotTransform) { //no sample available, return return false; } if(doInterpolation) { double timeForward = (atTime - lastTransformTime).toSeconds(); if(timeForward < 0) { throw std::runtime_error("Error, time of sample is lower than transformation time"); } if(timeForward == 0) { //transform time is equal to sample time, no interpolation needed result = lastTransform; return true; } std::pair<base::Time, TransformationType> next_sample; if(!aggregator.getNextSample(streamIdx, next_sample)) { //std::cout << "Transformer: could not get next sample" << std::endl; //not enought samples for itnerpolation available return false; } TransformationType interpolated; interpolated.initSane(); double timeBetweenTransforms = (next_sample.first - lastTransformTime).toSeconds(); assert(timeBetweenTransforms > timeForward); double factor = timeForward / timeBetweenTransforms; Eigen::Quaterniond start_r(lastTransform.orientation); Eigen::Quaterniond end_r(next_sample.second.orientation); interpolated.orientation = (start_r.slerp(factor, end_r)); Eigen::Vector3d start_t(lastTransform.position); Eigen::Vector3d end_t(next_sample.second.position); interpolated.position = factor * start_t + (1.0-factor) * end_t; // perform linear interpolation of uncertainties interpolated.cov_position = factor * lastTransform.cov_position + (1.0-factor) * next_sample.second.cov_position; interpolated.cov_orientation = factor * lastTransform.cov_orientation + (1.0-factor) * next_sample.second.cov_orientation; result = interpolated; } else { result = lastTransform; } return true; };