void CameraThread::process()
{
UTimer timer;
ULOGGER_DEBUG("Camera::process()");
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat img = _camera->takeImage(descriptors, keypoints);
if(!img.empty() && !this->isKilled())
{
if(_camera->isFeaturesExtracted())
{
this->post(new CameraEvent(descriptors, keypoints, img, ++_seq));
}
else
{
this->post(new CameraEvent(img, ++_seq));
}
}
else if(!this->isKilled())
{
if(_autoRestart)
{
_camera->init();
}
else
{
ULOGGER_DEBUG("Camera::process() : no more images...");
this->kill();
this->post(new CameraEvent());
}
}
}
void UThread::start()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
if(state_ == kSIdle || state_ == kSKilled)
{
if(state_ == kSKilled)
{
// make sure it is finished
runningMutex_.lock();
runningMutex_.unlock();
}
state_ = kSCreating;
int r = UThreadC<void>::Create(threadId_, &handle_, true); // Create detached
if(r)
{
UERROR("Failed to create a thread! errno=%d", r);
threadId_=0;
handle_=0;
state_ = kSIdle;
}
else
{
#if PRINT_DEBUG
ULOGGER_DEBUG("StateThread::startThread() thread id=%d _handle=%d", threadId_, handle_);
#endif
}
}
}
void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
{
ULOGGER_DEBUG("");
std::list<VisualWord *> toSave;
std::list<VisualWord *> toUpdate;
if(this->isConnected() && words.size())
{
for(std::vector<VisualWord *>::const_iterator i=words.begin(); i!=words.end();++i)
{
if((*i)->isSaved())
{
toUpdate.push_back(*i);
}
else
{
toSave.push_back(*i);
}
}
if(toUpdate.size())
{
this->updateQuery(toUpdate, _timestampUpdate);
}
if(toSave.size())
{
this->saveQuery(toSave);
}
}
}
void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
{
ULOGGER_DEBUG("");
std::map<int, float> newPosterior;
for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
{
std::map<int, float>::iterator post = _posterior.find(*i);
if(post == _posterior.end())
{
if(_posterior.size() == 0)
{
newPosterior.insert(std::pair<int, float>(*i, 1));
}
else
{
newPosterior.insert(std::pair<int, float>(*i, 0));
}
}
else
{
newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
}
}
_posterior = newPosterior;
}
void CameraThread::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
}
_stateMutex.unlock();
}
void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap)
{
ULOGGER_DEBUG("dataMap=%d, weightsMap=%d", dataMap.size(), weightsMap.size());
if(dataMap.size() > 0)
{
//match the size of the current data
int margin = int((_items.size()+1)/2) - dataMap.size();
while(margin < 0)
{
PdfPlotItem * newItem = new PdfPlotItem(0, 0, 2, 0);
newItem->setImagesRef(_imagesMapRef);
this->_addValue(newItem);
++margin;
}
while(margin > 0)
{
this->removeItem(0);
--margin;
}
ULOGGER_DEBUG("itemsize=%d", _items.size());
// update values
QList<QGraphicsItem*>::iterator iter = _items.begin();
QMap<int, int>::const_iterator j=weightsMap.begin();
for(QMap<int, float>::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i, ++j)
{
((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), j!=weightsMap.end()?j.value():-1);
//2 times...
++iter;
++iter;
}
//reset minMax, this will force the plot to update the axes
this->updateMinMax();
emit dataChanged(this);
}
}
bool CameraVideo::init()
{
if(_capture.isOpened())
{
_capture.release();
}
if(_src == kUsbDevice)
{
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h);
_capture.open(_usbDevice);
if(w && h)
{
_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w));
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h));
}
}
else if(_src == kVideoFile)
{
ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
_capture.open(_filePath.c_str());
}
else
{
ULOGGER_ERROR("Camera: Unknown source...");
}
if(!_capture.isOpened())
{
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
}
_stateMutex.unlock();
_dataAdded.release();
}
void UThread::ThreadMain()
{
runningMutex_.lock();
applyPriority();
applyAffinity();
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoopBegin()");
#endif
state_ = kSRunning;
mainLoopBegin();
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoop()");
#endif
while(state_ == kSRunning)
{
mainLoop();
}
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoopEnd()");
#endif
mainLoopEnd();
handle_ = 0;
threadId_ = 0;
state_ = kSIdle;
runningMutex_.unlock();
#if PRINT_DEBUG
ULOGGER_DEBUG("Exiting thread loop");
#endif
}
void Camera::parseParameters(const ParametersMap & parameters)
{
UDEBUG("");
ParametersMap::const_iterator iter;
//Keypoint detector
KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
}
if(detector!=KeypointDetector::kDetectorUndef)
{
ULOGGER_DEBUG("new detector strategy %d", int(detector));
if(_keypointDetector)
{
delete _keypointDetector;
_keypointDetector = 0;
}
if(_keypointDescriptor)
{
delete _keypointDescriptor;
_keypointDescriptor = 0;
}
switch(detector)
{
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
_keypointDescriptor = new SURFDescriptor(parameters);
break;
}
}
else
{
if(_keypointDetector)
{
_keypointDetector->parseParameters(parameters);
}
if(_keypointDescriptor)
{
_keypointDescriptor->parseParameters(parameters);
}
}
}
bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
{
if(_capture.isOpened())
{
_capture.release();
}
ULOGGER_DEBUG("Camera::init()");
_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
if(_capture.isOpened())
{
_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
{
UERROR("Depth registration is not activated on this device!");
}
if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
{
UINFO("Image generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
}
else
{
UERROR("Camera: Device doesn't contain image generator.");
_capture.release();
return false;
}
}
else
{
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::stringstream query;
query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
<< stMemSize << ","
<< lastSignAdded << ","
<< processMemUsed << ","
<< databaseMemUsed << ","
<< dictionarySize << ");";
this->executeNoResultQuery(query.str());
}
}
void CameraThread::handleEvent(UEvent* anEvent)
{
if(anEvent->getClassName().compare("ParamEvent") == 0)
{
if(this->isIdle())
{
_stateMutex.lock();
_camera->parseParameters(((ParamEvent*)anEvent)->getParameters());
_stateMutex.unlock();
}
else
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)anEvent)->getParameters());
}
}
}
void RtabmapThread::getData(SensorData & image)
{
ULOGGER_DEBUG("");
ULOGGER_INFO("waiting for data");
_dataAdded.acquire();
ULOGGER_INFO("wake-up");
_dataMutex.lock();
{
if(!_dataBuffer.empty())
{
image = _dataBuffer.front();
_dataBuffer.pop_front();
}
}
_dataMutex.unlock();
}
//============================================================
// MAIN LOOP
//============================================================
void RtabmapThread::process()
{
SensorData data;
getData(data);
if(data.isValid())
{
if(_rtabmap->getMemory())
{
if(_rtabmap->process(data))
{
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
}
}
else
{
UERROR("RTAB-Map is not initialized! Ignoring received data...");
}
}
}
void UThread::kill()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
killSafelyMutex_.lock();
{
if(this->isRunning())
{
// Thread is creating
while(state_ == kSCreating)
{
uSleep(1);
}
if(state_ == kSRunning)
{
state_ = kSKilled;
// Call function to do something before wait
mainLoopKill();
}
else
{
UERROR("thread (%d) is supposed to be running...", threadId_);
}
}
else
{
#if PRINT_DEBUG
UDEBUG("thread (%d) is not running...", threadId_);
#endif
}
}
killSafelyMutex_.unlock();
}
UThread::~UThread()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
}
cv::Mat CameraImages::captureImage()
{
UDEBUG("");
cv::Mat img;
if(_dir->isValid())
{
if(_refreshDir)
{
_dir->update();
}
if(_startAt == 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
if(fileNames.size())
{
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cv::imread(fullPath.c_str());
}
}
}
else
{
std::string fileName;
std::string fullPath;
fileName = _dir->getNextFileName();
if(fileName.size())
{
fullPath = _path + fileName;
while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
{
fullPath = _path + fileName;
}
if(fileName.size())
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", img.cols, img.rows, img.channels(), img.elemSize(), img.total());
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
}
}
}
}
else
{
UWARN("Directory is not set, camera must be initialized.");
}
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
if(!img.empty() &&
w &&
h &&
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
{
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
}
return img;
}
void DBDriver::commit() const
{
ULOGGER_DEBUG("");
this->executeNoResultQuery("COMMIT;");
_transactionMutex.unlock();
}
void DBDriver::beginTransaction() const
{
_transactionMutex.lock();
ULOGGER_DEBUG("");
this->executeNoResultQuery("BEGIN TRANSACTION;");
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
{
if(!_fullPredictionUpdate && !_prediction.empty())
{
return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
}
UDEBUG("");
UASSERT(memory &&
_predictionLC.size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
std::map<int, int> idToIndexMap;
for(unsigned int i=0; i<ids.size(); ++i)
{
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(idsDone.find(ids[i]) == idsDone.end())
{
if(ids[i] > 0)
{
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
{
if(memory->isInSTM(iter->first))
{
neighbors.erase(iter++);
}
else
{
if(iter->second == 0)
{
idsLoopMargin.push_back(iter->second);
}
++iter;
}
}
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, i, sum, ids[0]<0);
}
}
else
{
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
if(cols>1) // The first must be the virtual place
{
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
else
{
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
}
}
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return prediction;
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
if(!memory)
{
ULOGGER_ERROR("Memory is Null!");
return _posterior;
}
if(!likelihood.size())
{
ULOGGER_ERROR("likelihood is empty!");
return _posterior;
}
if(_predictionLC.size() < 2)
{
ULOGGER_ERROR("Prediction is not valid!");
return _posterior;
}
UTimer timer;
timer.start();
cv::Mat prior;
cv::Mat posterior;
float sum = 0;
int j=0;
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
_prediction = this->generatePrediction(memory, uKeys(likelihood));
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
//std::cout << "Prediction=" << _prediction << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
this->updatePosterior(memory, uKeys(likelihood));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
((float*)posterior.data)[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = _prediction * posterior;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << prior << std::endl;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
std::vector<float> likelihoodValues = uValues(likelihood);
//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
// STEP 2 - Update : Multiply with observations (likelihood)
j=0;
for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
{
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
{
ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
}
}
ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
//std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
// Normalize
ULOGGER_DEBUG("sum=%f", sum);
if(sum != 0)
{
for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
(*i).second /= sum;
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
//std::cout << "Posterior=" << _posterior << std::endl;
return _posterior;
}
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());
}
}
void DBDriver::emptyTrashes(bool async)
{
if(async)
{
ULOGGER_DEBUG("Async emptying, start the trash thread");
this->start();
return;
}
UTimer totalTime;
totalTime.start();
std::map<int, Signature*> signatures;
std::map<int, VisualWord*> visualWords;
_trashesMutex.lock();
{
ULOGGER_DEBUG("signatures=%d, visualWords=%d", _trashSignatures.size(), _trashVisualWords.size());
signatures = _trashSignatures;
visualWords = _trashVisualWords;
_trashSignatures.clear();
_trashVisualWords.clear();
_dbSafeAccessMutex.lock();
}
_trashesMutex.unlock();
if(signatures.size() || visualWords.size())
{
this->beginTransaction();
UTimer timer;
timer.start();
if(signatures.size())
{
if(this->isConnected())
{
//Only one query to the database
this->saveOrUpdate(uValues(signatures));
}
for(std::map<int, Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
delete iter->second;
}
signatures.clear();
ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
}
if(visualWords.size())
{
if(this->isConnected())
{
//Only one query to the database
this->saveOrUpdate(uValues(visualWords));
}
for(std::map<int, VisualWord *>::iterator iter=visualWords.begin(); iter!=visualWords.end(); ++iter)
{
delete (*iter).second;
}
visualWords.clear();
ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
}
this->commit();
}
_emptyTrashesTime = totalTime.ticks();
ULOGGER_DEBUG("Total time emptying trashes = %fs...", _emptyTrashesTime);
_dbSafeAccessMutex.unlock();
}
