bool DataRecorder::init(const QString & path, bool recordInRAM)
{
UScopeMutex scope(memoryMutex_);
if(!memory_)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
if(!recordInRAM)
{
customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
}
memory_ = new Memory();
if(!memory_->init(path.toStdString(), true, customParameters))
{
delete memory_;
memory_ = 0;
UERROR("Error initializing the memory.");
return false;
}
path_ = path;
return true;
}
else
{
UERROR("Already initialized, close it first.");
return false;
}
}
void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
{
memoryMutex_.lock();
if(memory_)
{
if(memory_->getStMem().size() == 0 && data.id() > 0)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
memory_->parseParameters(customParameters);
}
//save to database
UTimer time;
memory_->update(data, pose, covariance);
const Signature * s = memory_->getLastWorkingSignature();
totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().laserScanCompressed().total()/1000;
memory_->cleanup();
if(++count_ % 30)
{
memory_->emptyTrash();
}
UDEBUG("Time to process a message = %f s", time.ticks());
}
memoryMutex_.unlock();
}
void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector, KeypointDescriptor::DescriptorType descriptor)
{
_featuresExtracted = featuresExtracted;
if(detector != KeypointDetector::kDetectorUndef || descriptor != KeypointDescriptor::kDescriptorUndef)
{
ParametersMap pm;
pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
this->parseParameters(pm);
}
}
void RtabmapThread::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
{
this->addData(e->data());
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
{
UDEBUG("OdometryEvent");
OdometryEvent * e = (OdometryEvent*)event;
if(e->isValid())
{
this->addData(e->data());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdInit)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(!rtabmapEvent->getStr().empty());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
pushNewState(kStateInit, parameters);
}
else if(cmd == RtabmapEventCmd::kCmdClose)
{
ULOGGER_DEBUG("CMD_CLOSE");
pushNewState(kStateClose);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingDOTGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
{
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
UASSERT(values.size() == 3);
ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingDOTLocalGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraphLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraphGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMapLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMapGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
{
ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingTOROGraphLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
{
ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingTOROGraphGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
pushNewState(kStateTriggeringMap);
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else
{
UWARN("Cmd %d unknown!", cmd);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
}
}