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