DBDriver.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/core/DBDriver.h"
00029 
00030 #include "rtabmap/core/Signature.h"
00031 #include "rtabmap/core/VisualWord.h"
00032 #include "rtabmap/utilite/UConversion.h"
00033 #include "rtabmap/utilite/UMath.h"
00034 #include "rtabmap/utilite/ULogger.h"
00035 #include "rtabmap/utilite/UTimer.h"
00036 #include "rtabmap/utilite/UStl.h"
00037 #include "DBDriverSqlite3.h"
00038 
00039 namespace rtabmap {
00040 
00041 DBDriver * DBDriver::create(const ParametersMap & parameters)
00042 {
00043         // well, we only have Sqlite3 database type for now :P
00044         return new DBDriverSqlite3(parameters);
00045 }
00046 
00047 DBDriver::DBDriver(const ParametersMap & parameters) :
00048         _emptyTrashesTime(0),
00049         _timestampUpdate(true)
00050 {
00051         this->parseParameters(parameters);
00052 }
00053 
00054 DBDriver::~DBDriver()
00055 {
00056         join(true);
00057         this->emptyTrashes();
00058 }
00059 
00060 void DBDriver::parseParameters(const ParametersMap & parameters)
00061 {
00062 }
00063 
00064 void DBDriver::closeConnection(bool save)
00065 {
00066         UDEBUG("isRunning=%d", this->isRunning());
00067         this->join(true);
00068         UDEBUG("");
00069         if(save)
00070         {
00071                 this->emptyTrashes();
00072         }
00073         else
00074         {
00075                 _trashesMutex.lock();
00076                 _trashSignatures.clear();
00077                 _trashVisualWords.clear();
00078                 _trashesMutex.unlock();
00079         }
00080         _dbSafeAccessMutex.lock();
00081         this->disconnectDatabaseQuery(save);
00082         _dbSafeAccessMutex.unlock();
00083         UDEBUG("");
00084 }
00085 
00086 bool DBDriver::openConnection(const std::string & url, bool overwritten)
00087 {
00088         UDEBUG("");
00089         _url = url;
00090         _dbSafeAccessMutex.lock();
00091         if(this->connectDatabaseQuery(url, overwritten))
00092         {
00093                 _dbSafeAccessMutex.unlock();
00094                 return true;
00095         }
00096         _dbSafeAccessMutex.unlock();
00097         return false;
00098 }
00099 
00100 bool DBDriver::isConnected() const
00101 {
00102         bool r;
00103         _dbSafeAccessMutex.lock();
00104         r = isConnectedQuery();
00105         _dbSafeAccessMutex.unlock();
00106         return r;
00107 }
00108 
00109 // In bytes
00110 long DBDriver::getMemoryUsed() const
00111 {
00112         long bytes;
00113         _dbSafeAccessMutex.lock();
00114         bytes = getMemoryUsedQuery();
00115         _dbSafeAccessMutex.unlock();
00116         return bytes;
00117 }
00118 
00119 long DBDriver::getImagesMemoryUsed() const
00120 {
00121         long bytes;
00122         _dbSafeAccessMutex.lock();
00123         bytes = getImagesMemoryUsedQuery();
00124         _dbSafeAccessMutex.unlock();
00125         return bytes;
00126 }
00127 long DBDriver::getDepthImagesMemoryUsed() const
00128 {
00129         long bytes;
00130         _dbSafeAccessMutex.lock();
00131         bytes = getDepthImagesMemoryUsedQuery();
00132         _dbSafeAccessMutex.unlock();
00133         return bytes;
00134 }
00135 long DBDriver::getLaserScansMemoryUsed() const
00136 {
00137         long bytes;
00138         _dbSafeAccessMutex.lock();
00139         bytes = getLaserScansMemoryUsedQuery();
00140         _dbSafeAccessMutex.unlock();
00141         return bytes;
00142 }
00143 long DBDriver::getUserDataMemoryUsed() const
00144 {
00145         long bytes;
00146         _dbSafeAccessMutex.lock();
00147         bytes = getUserDataMemoryUsedQuery();
00148         _dbSafeAccessMutex.unlock();
00149         return bytes;
00150 }
00151 long DBDriver::getWordsMemoryUsed() const
00152 {
00153         long bytes;
00154         _dbSafeAccessMutex.lock();
00155         bytes = getWordsMemoryUsedQuery();
00156         _dbSafeAccessMutex.unlock();
00157         return bytes;
00158 }
00159 int DBDriver::getLastNodesSize() const
00160 {
00161         int nodes;
00162         _dbSafeAccessMutex.lock();
00163         nodes = getLastNodesSizeQuery();
00164         _dbSafeAccessMutex.unlock();
00165         return nodes;
00166 }
00167 int DBDriver::getLastDictionarySize() const
00168 {
00169         int words;
00170         _dbSafeAccessMutex.lock();
00171         words = getLastDictionarySizeQuery();
00172         _dbSafeAccessMutex.unlock();
00173         return words;
00174 }
00175 int DBDriver::getTotalNodesSize() const
00176 {
00177         int words;
00178         _dbSafeAccessMutex.lock();
00179         words = getTotalNodesSizeQuery();
00180         _dbSafeAccessMutex.unlock();
00181         return words;
00182 }
00183 int DBDriver::getTotalDictionarySize() const
00184 {
00185         int words;
00186         _dbSafeAccessMutex.lock();
00187         words = getTotalDictionarySizeQuery();
00188         _dbSafeAccessMutex.unlock();
00189         return words;
00190 }
00191 ParametersMap DBDriver::getLastParameters() const
00192 {
00193         ParametersMap parameters;
00194         _dbSafeAccessMutex.lock();
00195         parameters = getLastParametersQuery();
00196         _dbSafeAccessMutex.unlock();
00197         return parameters;
00198 }
00199 
00200 std::string DBDriver::getDatabaseVersion() const
00201 {
00202         std::string version = "0.0.0";
00203         _dbSafeAccessMutex.lock();
00204         getDatabaseVersionQuery(version);
00205         _dbSafeAccessMutex.unlock();
00206         return version;
00207 }
00208 
00209 void DBDriver::mainLoop()
00210 {
00211         this->emptyTrashes();
00212         this->kill(); // Do it only once
00213 }
00214 
00215 void DBDriver::beginTransaction() const
00216 {
00217         _transactionMutex.lock();
00218         ULOGGER_DEBUG("");
00219         this->executeNoResultQuery("BEGIN TRANSACTION;");
00220 }
00221 
00222 void DBDriver::commit() const
00223 {
00224         ULOGGER_DEBUG("");
00225         this->executeNoResultQuery("COMMIT;");
00226         _transactionMutex.unlock();
00227 }
00228 
00229 void DBDriver::executeNoResult(const std::string & sql) const
00230 {
00231         _dbSafeAccessMutex.lock();
00232         this->executeNoResultQuery(sql);
00233         _dbSafeAccessMutex.unlock();
00234 }
00235 
00236 void DBDriver::emptyTrashes(bool async)
00237 {
00238         if(async)
00239         {
00240                 ULOGGER_DEBUG("Async emptying, start the trash thread");
00241                 this->start();
00242                 return;
00243         }
00244 
00245         UTimer totalTime;
00246         totalTime.start();
00247 
00248         std::map<int, Signature*> signatures;
00249         std::map<int, VisualWord*> visualWords;
00250         _trashesMutex.lock();
00251         {
00252                 ULOGGER_DEBUG("signatures=%d, visualWords=%d", _trashSignatures.size(), _trashVisualWords.size());
00253                 signatures = _trashSignatures;
00254                 visualWords = _trashVisualWords;
00255                 _trashSignatures.clear();
00256                 _trashVisualWords.clear();
00257 
00258                 _dbSafeAccessMutex.lock();
00259         }
00260         _trashesMutex.unlock();
00261 
00262         if(signatures.size() || visualWords.size())
00263         {
00264                 this->beginTransaction();
00265                 UTimer timer;
00266                 timer.start();
00267                 if(signatures.size())
00268                 {
00269                         if(this->isConnected())
00270                         {
00271                                 //Only one query to the database
00272                                 this->saveOrUpdate(uValues(signatures));
00273                         }
00274 
00275                         for(std::map<int, Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
00276                         {
00277                                 delete iter->second;
00278                         }
00279                         signatures.clear();
00280                         ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
00281                 }
00282                 if(visualWords.size())
00283                 {
00284                         if(this->isConnected())
00285                         {
00286                                 //Only one query to the database
00287                                 this->saveOrUpdate(uValues(visualWords));
00288                         }
00289 
00290                         for(std::map<int, VisualWord *>::iterator iter=visualWords.begin(); iter!=visualWords.end(); ++iter)
00291                         {
00292                                 delete (*iter).second;
00293                         }
00294                         visualWords.clear();
00295                         ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
00296                 }
00297 
00298                 this->commit();
00299         }
00300 
00301         _emptyTrashesTime = totalTime.ticks();
00302         ULOGGER_DEBUG("Total time emptying trashes = %fs...", _emptyTrashesTime);
00303 
00304         _dbSafeAccessMutex.unlock();
00305 }
00306 
00307 void DBDriver::asyncSave(Signature * s)
00308 {
00309         if(s)
00310         {
00311                 UDEBUG("s=%d", s->id());
00312                 _trashesMutex.lock();
00313                 {
00314                         _trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
00315                 }
00316                 _trashesMutex.unlock();
00317         }
00318 }
00319 
00320 void DBDriver::asyncSave(VisualWord * vw)
00321 {
00322         if(vw)
00323         {
00324                 _trashesMutex.lock();
00325                 {
00326                         _trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
00327                 }
00328                 _trashesMutex.unlock();
00329         }
00330 }
00331 
00332 void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
00333 {
00334         ULOGGER_DEBUG("");
00335         std::list<Signature *> toSave;
00336         std::list<Signature *> toUpdate;
00337         if(this->isConnected() && signatures.size())
00338         {
00339                 for(std::vector<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end();++i)
00340                 {
00341                         if((*i)->isSaved())
00342                         {
00343                                 toUpdate.push_back(*i);
00344                         }
00345                         else
00346                         {
00347                                 toSave.push_back(*i);
00348                         }
00349                 }
00350 
00351                 if(toUpdate.size())
00352                 {
00353                         this->updateQuery(toUpdate, _timestampUpdate);
00354                 }
00355                 if(toSave.size())
00356                 {
00357                         this->saveQuery(toSave);
00358                 }
00359         }
00360 }
00361 
00362 void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
00363 {
00364         ULOGGER_DEBUG("");
00365         std::list<VisualWord *> toSave;
00366         std::list<VisualWord *> toUpdate;
00367         if(this->isConnected() && words.size())
00368         {
00369                 for(std::vector<VisualWord *>::const_iterator i=words.begin(); i!=words.end();++i)
00370                 {
00371                         if((*i)->isSaved())
00372                         {
00373                                 toUpdate.push_back(*i);
00374                         }
00375                         else
00376                         {
00377                                 toSave.push_back(*i);
00378                         }
00379                 }
00380 
00381                 if(toUpdate.size())
00382                 {
00383                         this->updateQuery(toUpdate, _timestampUpdate);
00384                 }
00385                 if(toSave.size())
00386                 {
00387                         this->saveQuery(toSave);
00388                 }
00389         }
00390 }
00391 
00392 void DBDriver::addLink(const Link & link)
00393 {
00394         _dbSafeAccessMutex.lock();
00395         this->addLinkQuery(link);
00396         _dbSafeAccessMutex.unlock();
00397 }
00398 void DBDriver::removeLink(int from, int to)
00399 {
00400         this->executeNoResult(uFormat("DELETE FROM Link WHERE from_id=%d and to_id=%d", from, to).c_str());
00401 }
00402 void DBDriver::updateLink(const Link & link)
00403 {
00404         _dbSafeAccessMutex.lock();
00405         this->updateLinkQuery(link);
00406         _dbSafeAccessMutex.unlock();
00407 }
00408 
00409 void DBDriver::load(VWDictionary * dictionary) const
00410 {
00411         _dbSafeAccessMutex.lock();
00412         this->loadQuery(dictionary);
00413         _dbSafeAccessMutex.unlock();
00414 }
00415 
00416 void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
00417 {
00418         _dbSafeAccessMutex.lock();
00419         this->loadLastNodesQuery(signatures);
00420         _dbSafeAccessMutex.unlock();
00421 }
00422 
00423 void DBDriver::loadSignatures(const std::list<int> & signIds,
00424                 std::list<Signature *> & signatures,
00425                 std::set<int> * loadedFromTrash)
00426 {
00427         UDEBUG("");
00428         // look up in the trash before the database
00429         std::list<int> ids = signIds;
00430         std::list<Signature*>::iterator sIter;
00431         bool valueFound = false;
00432         _trashesMutex.lock();
00433         {
00434                 for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
00435                 {
00436                         valueFound = false;
00437                         for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
00438                         {
00439                                 if(sIter->first == *iter)
00440                                 {
00441                                         signatures.push_back(sIter->second);
00442                                         _trashSignatures.erase(sIter++);
00443 
00444                                         valueFound = true;
00445                                         break;
00446                                 }
00447                                 else
00448                                 {
00449                                         ++sIter;
00450                                 }
00451                         }
00452                         if(valueFound)
00453                         {
00454                                 if(loadedFromTrash)
00455                                 {
00456                                         loadedFromTrash->insert(*iter);
00457                                 }
00458                                 iter = ids.erase(iter);
00459                         }
00460                         else
00461                         {
00462                                 ++iter;
00463                         }
00464                 }
00465         }
00466         _trashesMutex.unlock();
00467         UDEBUG("");
00468         if(ids.size())
00469         {
00470                 _dbSafeAccessMutex.lock();
00471                 this->loadSignaturesQuery(ids, signatures);
00472                 _dbSafeAccessMutex.unlock();
00473         }
00474 }
00475 
00476 void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws)
00477 {
00478         // look up in the trash before the database
00479         std::set<int> ids = wordIds;
00480         std::map<int, VisualWord*>::iterator wIter;
00481         std::list<VisualWord *> puttedBack;
00482         _trashesMutex.lock();
00483         {
00484                 if(_trashVisualWords.size())
00485                 {
00486                         for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
00487                         {
00488                                 wIter = _trashVisualWords.find(*iter);
00489                                 if(wIter != _trashVisualWords.end())
00490                                 {
00491                                         UDEBUG("put back word %d from trash", *iter);
00492                                         puttedBack.push_back(wIter->second);
00493                                         _trashVisualWords.erase(wIter);
00494                                         ids.erase(iter++);
00495                                 }
00496                                 else
00497                                 {
00498                                         ++iter;
00499                                 }
00500                         }
00501                 }
00502         }
00503         _trashesMutex.unlock();
00504         if(ids.size())
00505         {
00506                 _dbSafeAccessMutex.lock();
00507                 this->loadWordsQuery(ids, vws);
00508                 _dbSafeAccessMutex.unlock();
00509                 uAppend(vws, puttedBack);
00510         }
00511         else if(puttedBack.size())
00512         {
00513                 uAppend(vws, puttedBack);
00514         }
00515 }
00516 
00517 void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
00518 {
00519         // Don't look in the trash, we assume that if we want to load
00520         // data of a signature, it is not in thrash! Print an error if so.
00521         _trashesMutex.lock();
00522         if(_trashSignatures.size())
00523         {
00524                 for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
00525                 {
00526                         UASSERT(*iter != 0);
00527                         UASSERT_MSG(!uContains(_trashSignatures, (*iter)->id()), uFormat("Signature %d should not be used when transferred to trash!!!!", (*iter)->id()).c_str());
00528                 }
00529         }
00530         _trashesMutex.unlock();
00531 
00532         _dbSafeAccessMutex.lock();
00533         this->loadNodeDataQuery(signatures);
00534         _dbSafeAccessMutex.unlock();
00535 }
00536 
00537 void DBDriver::getNodeData(
00538                 int signatureId,
00539                 SensorData & data) const
00540 {
00541         bool found = false;
00542         // look in the trash
00543         _trashesMutex.lock();
00544         if(uContains(_trashSignatures, signatureId))
00545         {
00546                 const Signature * s = _trashSignatures.at(signatureId);
00547                 if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
00548                 {
00549                         data = (SensorData)s->sensorData();
00550                         found = true;
00551                 }
00552         }
00553         _trashesMutex.unlock();
00554 
00555         if(!found)
00556         {
00557                 _dbSafeAccessMutex.lock();
00558                 std::list<Signature *> signatures;
00559                 Signature tmp(signatureId);
00560                 signatures.push_back(&tmp);
00561                 loadNodeDataQuery(signatures);
00562                 data = signatures.front()->sensorData();
00563                 _dbSafeAccessMutex.unlock();
00564         }
00565 }
00566 
00567 bool DBDriver::getCalibration(
00568                 int signatureId,
00569                 std::vector<CameraModel> & models,
00570                 StereoCameraModel & stereoModel) const
00571 {
00572         bool found = false;
00573         // look in the trash
00574         _trashesMutex.lock();
00575         if(uContains(_trashSignatures, signatureId))
00576         {
00577                 models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
00578                 stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
00579                 found = true;
00580         }
00581         _trashesMutex.unlock();
00582 
00583         if(!found)
00584         {
00585                 _dbSafeAccessMutex.lock();
00586                 found = this->getCalibrationQuery(signatureId, models, stereoModel);
00587                 _dbSafeAccessMutex.unlock();
00588         }
00589         return found;
00590 }
00591 
00592 bool DBDriver::getNodeInfo(
00593                 int signatureId,
00594                 Transform & pose,
00595                 int & mapId,
00596                 int & weight,
00597                 std::string & label,
00598                 double & stamp,
00599                 Transform & groundTruthPose) const
00600 {
00601         bool found = false;
00602         // look in the trash
00603         _trashesMutex.lock();
00604         if(uContains(_trashSignatures, signatureId))
00605         {
00606                 pose = _trashSignatures.at(signatureId)->getPose();
00607                 mapId = _trashSignatures.at(signatureId)->mapId();
00608                 weight = _trashSignatures.at(signatureId)->getWeight();
00609                 label = _trashSignatures.at(signatureId)->getLabel();
00610                 stamp = _trashSignatures.at(signatureId)->getStamp();
00611                 groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
00612                 found = true;
00613         }
00614         _trashesMutex.unlock();
00615 
00616         if(!found)
00617         {
00618                 _dbSafeAccessMutex.lock();
00619                 found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose);
00620                 _dbSafeAccessMutex.unlock();
00621         }
00622         return found;
00623 }
00624 
00625 void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
00626 {
00627         bool found = false;
00628         // look in the trash
00629         _trashesMutex.lock();
00630         if(uContains(_trashSignatures, signatureId))
00631         {
00632                 const Signature * s = _trashSignatures.at(signatureId);
00633                 UASSERT(s != 0);
00634                 for(std::map<int, Link>::const_iterator nIter = s->getLinks().begin();
00635                                 nIter!=s->getLinks().end();
00636                                 ++nIter)
00637                 {
00638                         if(type == Link::kUndef || nIter->second.type() == type)
00639                         {
00640                                 links.insert(*nIter);
00641                         }
00642                 }
00643                 found = true;
00644         }
00645         _trashesMutex.unlock();
00646 
00647         if(!found)
00648         {
00649                 _dbSafeAccessMutex.lock();
00650                 this->loadLinksQuery(signatureId, links, type);
00651                 _dbSafeAccessMutex.unlock();
00652         }
00653 }
00654 
00655 void DBDriver::getWeight(int signatureId, int & weight) const
00656 {
00657         bool found = false;
00658         // look in the trash
00659         _trashesMutex.lock();
00660         if(uContains(_trashSignatures, signatureId))
00661         {
00662                 weight = _trashSignatures.at(signatureId)->getWeight();
00663                 found = true;
00664         }
00665         _trashesMutex.unlock();
00666 
00667         if(!found)
00668         {
00669                 _dbSafeAccessMutex.lock();
00670                 this->getWeightQuery(signatureId, weight);
00671                 _dbSafeAccessMutex.unlock();
00672         }
00673 }
00674 
00675 void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
00676 {
00677         // look in the trash
00678         _trashesMutex.lock();
00679         if(_trashSignatures.size())
00680         {
00681                 for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
00682                 {
00683                         bool hasNeighbors = !ignoreChildren;
00684                         if(ignoreChildren)
00685                         {
00686                                 for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
00687                                                 nIter!=sIter->second->getLinks().end();
00688                                                 ++nIter)
00689                                 {
00690                                         if(nIter->second.type() == Link::kNeighbor ||
00691                                            nIter->second.type() == Link::kNeighborMerged)
00692                                         {
00693                                                 hasNeighbors = true;
00694                                                 break;
00695                                         }
00696                                 }
00697                         }
00698                         if(hasNeighbors)
00699                         {
00700                                 ids.insert(sIter->first);
00701                         }
00702                 }
00703 
00704                 std::vector<int> keys = uKeys(_trashSignatures);
00705 
00706         }
00707         _trashesMutex.unlock();
00708 
00709         _dbSafeAccessMutex.lock();
00710         this->getAllNodeIdsQuery(ids, ignoreChildren, ignoreBadSignatures);
00711         _dbSafeAccessMutex.unlock();
00712 }
00713 
00714 void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks) const
00715 {
00716         _dbSafeAccessMutex.lock();
00717         this->getAllLinksQuery(links, ignoreNullLinks);
00718         _dbSafeAccessMutex.unlock();
00719 
00720         // look in the trash
00721         _trashesMutex.lock();
00722         if(_trashSignatures.size())
00723         {
00724                 for(std::map<int, Signature*>::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
00725                 {
00726                         links.erase(iter->first);
00727                         for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
00728                                 jter!=iter->second->getLinks().end();
00729                                 ++jter)
00730                         {
00731                                 if(!ignoreNullLinks || jter->second.isValid())
00732                                 {
00733                                         links.insert(std::make_pair(iter->first, jter->second));
00734                                 }
00735                         }
00736                 }
00737         }
00738         _trashesMutex.unlock();
00739 }
00740 
00741 void DBDriver::getLastNodeId(int & id) const
00742 {
00743         // look in the trash
00744         _trashesMutex.lock();
00745         if(_trashSignatures.size())
00746         {
00747                 id = _trashSignatures.rbegin()->first;
00748         }
00749         _trashesMutex.unlock();
00750 
00751         _dbSafeAccessMutex.lock();
00752         this->getLastIdQuery("Node", id);
00753         _dbSafeAccessMutex.unlock();
00754 }
00755 
00756 void DBDriver::getLastWordId(int & id) const
00757 {
00758         // look in the trash
00759         _trashesMutex.lock();
00760         if(_trashVisualWords.size())
00761         {
00762                 id = _trashVisualWords.rbegin()->first;
00763         }
00764         _trashesMutex.unlock();
00765 
00766         _dbSafeAccessMutex.lock();
00767         this->getLastIdQuery("Word", id);
00768         _dbSafeAccessMutex.unlock();
00769 }
00770 
00771 void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
00772 {
00773         bool found = false;
00774         // look in the trash
00775         _trashesMutex.lock();
00776         if(uContains(_trashSignatures, signatureId))
00777         {
00778                 ni = _trashSignatures.at(signatureId)->getWords().size();
00779                 found = true;
00780         }
00781         _trashesMutex.unlock();
00782 
00783         if(!found)
00784         {
00785                 _dbSafeAccessMutex.lock();
00786                 this->getInvertedIndexNiQuery(signatureId, ni);
00787                 _dbSafeAccessMutex.unlock();
00788         }
00789 }
00790 
00791 void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
00792 {
00793         if(!label.empty())
00794         {
00795                 int idFound = 0;
00796                 // look in the trash
00797                 _trashesMutex.lock();
00798                 for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
00799                 {
00800                         if(sIter->second->getLabel().compare(label) == 0)
00801                         {
00802                                 idFound = sIter->first;
00803                                 break;
00804                         }
00805                 }
00806                 _trashesMutex.unlock();
00807 
00808                 // then look in the database
00809                 if(idFound == 0)
00810                 {
00811                         _dbSafeAccessMutex.lock();
00812                         this->getNodeIdByLabelQuery(label, id);
00813                         _dbSafeAccessMutex.unlock();
00814                 }
00815                 else
00816                 {
00817                         id = idFound;
00818                 }
00819         }
00820         else
00821         {
00822                 UWARN("Can't search with an empty label!");
00823         }
00824 }
00825 
00826 void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
00827 {
00828         // look in the trash
00829         _trashesMutex.lock();
00830         for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
00831         {
00832                 if(!sIter->second->getLabel().empty())
00833                 {
00834                         labels.insert(std::make_pair(sIter->first, sIter->second->getLabel()));
00835                 }
00836         }
00837         _trashesMutex.unlock();
00838 
00839         // then look in the database
00840         _dbSafeAccessMutex.lock();
00841         this->getAllLabelsQuery(labels);
00842         _dbSafeAccessMutex.unlock();
00843 }
00844 
00845 void DBDriver::addStatisticsAfterRun(
00846                 int stMemSize,
00847                 int lastSignAdded,
00848                 int processMemUsed,
00849                 int databaseMemUsed,
00850                 int dictionarySize,
00851                 const ParametersMap & parameters) const
00852 {
00853         ULOGGER_DEBUG("");
00854         if(this->isConnected())
00855         {
00856                 std::stringstream query;
00857                 if(uStrNumCmp(this->getDatabaseVersion(), "0.11.8") >= 0)
00858                 {
00859                         std::string param = Parameters::serialize(parameters);
00860                         query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size,parameters) values("
00861                                   << stMemSize << ","
00862                                   << lastSignAdded << ","
00863                                   << processMemUsed << ","
00864                                   << databaseMemUsed << ","
00865                                   << dictionarySize << ","
00866                                   "\"" << param.c_str() << "\");";
00867                 }
00868                 else
00869                 {
00870                         query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
00871                                   << stMemSize << ","
00872                                   << lastSignAdded << ","
00873                                   << processMemUsed << ","
00874                                   << databaseMemUsed << ","
00875                                   << dictionarySize << ");";
00876                 }
00877 
00878                 this->executeNoResultQuery(query.str());
00879         }
00880 }
00881 
00882 void DBDriver::generateGraph(
00883                 const std::string & fileName,
00884                 const std::set<int> & idsInput,
00885                 const std::map<int, Signature *> & otherSignatures)
00886 {
00887         if(this->isConnected())
00888         {
00889                 if(!fileName.empty())
00890                 {
00891                         FILE* fout = 0;
00892                         #ifdef _MSC_VER
00893                                 fopen_s(&fout, fileName.c_str(), "w");
00894                         #else
00895                                 fout = fopen(fileName.c_str(), "w");
00896                         #endif
00897 
00898                          if (!fout)
00899                          {
00900                                  UERROR("Cannot open file %s!", fileName.c_str());
00901                                  return;
00902                          }
00903 
00904                          std::set<int> ids;
00905                          if(idsInput.size() == 0)
00906                          {
00907                                  this->getAllNodeIds(ids);
00908                                  UDEBUG("ids.size()=%d", ids.size());
00909                                  for(std::map<int, Signature*>::const_iterator iter=otherSignatures.begin(); iter!=otherSignatures.end(); ++iter)
00910                                  {
00911                                          ids.insert(iter->first);
00912                                  }
00913                          }
00914                          else
00915                          {
00916                                  ids = idsInput;
00917                          }
00918 
00919                          const char * colorG = "green";
00920                          const char * colorP = "pink";
00921                          const char * colorNM = "blue";
00922                          UINFO("Generating map with %d locations", ids.size());
00923                          fprintf(fout, "digraph G {\n");
00924                          for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
00925                          {
00926                                  if(otherSignatures.find(*i) == otherSignatures.end())
00927                                  {
00928                                          int id = *i;
00929                                          std::map<int, Link> links;
00930                                          this->loadLinks(id, links);
00931                                          int weight = 0;
00932                                          this->getWeight(id, weight);
00933                                          for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
00934                                          {
00935                                                  int weightNeighbor = 0;
00936                                                  if(otherSignatures.find(iter->first) == otherSignatures.end())
00937                                                  {
00938                                                          this->getWeight(iter->first, weightNeighbor);
00939                                                  }
00940                                                  else
00941                                                  {
00942                                                          weightNeighbor = otherSignatures.find(iter->first)->second->getWeight();
00943                                                  }
00944                                                  //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
00945                                                  if(iter->second.type() == Link::kNeighbor)
00946                                                  {
00947                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
00948                                                                          id,
00949                                                                          weight,
00950                                                                          iter->first,
00951                                                                          weightNeighbor);
00952                                                  }
00953                                                  else if(iter->second.type() == Link::kNeighborMerged)
00954                                                  {
00955                                                          //merged neighbor
00956                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"M\", fontcolor=%s, fontsize=8];\n",
00957                                                                          id,
00958                                                                          weight,
00959                                                                          iter->first,
00960                                                                          weightNeighbor,
00961                                                                          colorNM);
00962                                                  }
00963                                                  else if(iter->first > id)
00964                                                  {
00965                                                          //loop
00966                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
00967                                                                          id,
00968                                                                          weight,
00969                                                                          iter->first,
00970                                                                          weightNeighbor,
00971                                                                          colorG);
00972                                                  }
00973                                                  else
00974                                                  {
00975                                                          //child
00976                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
00977                                                                          id,
00978                                                                          weight,
00979                                                                          iter->first,
00980                                                                          weightNeighbor,
00981                                                                          colorP);
00982                                                  }
00983                                          }
00984                                  }
00985                          }
00986                          for(std::map<int, Signature*>::const_iterator i=otherSignatures.begin(); i!=otherSignatures.end(); ++i)
00987                          {
00988                                  if(ids.find(i->first) != ids.end())
00989                                  {
00990                                          int id = i->second->id();
00991                                          const std::map<int, Link> & links = i->second->getLinks();
00992                                          int weight = i->second->getWeight();
00993                                          for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
00994                                          {
00995                                                  int weightNeighbor = 0;
00996                                                  const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0);
00997                                                  if(s)
00998                                                  {
00999                                                          weightNeighbor = s->getWeight();
01000                                                  }
01001                                                  else
01002                                                  {
01003                                                          this->getWeight(iter->first, weightNeighbor);
01004                                                  }
01005                                                  //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
01006                                                  if(iter->second.type() == Link::kNeighbor)
01007                                                  {
01008                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
01009                                                                          id,
01010                                                                          weight,
01011                                                                          iter->first,
01012                                                                          weightNeighbor);
01013                                                  }
01014                                                  else if(iter->second.type() == Link::kNeighborMerged)
01015                                                  {
01016                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"M\", fontcolor=%s, fontsize=8];\n",
01017                                                                          id,
01018                                                                          weight,
01019                                                                          iter->first,
01020                                                                          weightNeighbor,
01021                                                                          colorNM);
01022                                                  }
01023                                                  else if(iter->first > id)
01024                                                  {
01025                                                          //loop
01026                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
01027                                                                          id,
01028                                                                          weight,
01029                                                                          iter->first,
01030                                                                          weightNeighbor,
01031                                                                          colorG);
01032                                                  }
01033                                                  else
01034                                                  {
01035                                                          //child
01036                                                          fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
01037                                                                          id,
01038                                                                          weight,
01039                                                                          iter->first,
01040                                                                          weightNeighbor,
01041                                                                          colorP);
01042                                                  }
01043                                          }
01044                                  }
01045                          }
01046                          fprintf(fout, "}\n");
01047                          fclose(fout);
01048                          UINFO("Graph saved to \"%s\" (Tip: $ neato -Tpdf \"%s\" -o out.pdf)", fileName.c_str(), fileName.c_str());
01049                 }
01050         }
01051 }
01052 
01053 } // namespace rtabmap


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:15