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