00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "DBDriverSqlite3.h"
00029
00030 #include "rtabmap/core/Signature.h"
00031 #include "rtabmap/core/VisualWord.h"
00032 #include "rtabmap/core/VWDictionary.h"
00033 #include "rtabmap/core/util3d.h"
00034 #include "rtabmap/core/Compression.h"
00035 #include "DatabaseSchema_sql.h"
00036 #include <set>
00037
00038 #include "rtabmap/utilite/UtiLite.h"
00039
00040 namespace rtabmap {
00041
00042 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
00043 DBDriver(parameters),
00044 _ppDb(0),
00045 _version("0.0.0"),
00046 _dbInMemory(Parameters::defaultDbSqlite3InMemory()),
00047 _cacheSize(Parameters::defaultDbSqlite3CacheSize()),
00048 _journalMode(Parameters::defaultDbSqlite3JournalMode()),
00049 _synchronous(Parameters::defaultDbSqlite3Synchronous()),
00050 _tempStore(Parameters::defaultDbSqlite3TempStore())
00051 {
00052 ULOGGER_DEBUG("treadSafe=%d", sqlite3_threadsafe());
00053 this->parseParameters(parameters);
00054 }
00055
00056 DBDriverSqlite3::~DBDriverSqlite3()
00057 {
00058 this->closeConnection();
00059 }
00060
00061 void DBDriverSqlite3::parseParameters(const ParametersMap & parameters)
00062 {
00063 ParametersMap::const_iterator iter;
00064 if((iter=parameters.find(Parameters::kDbSqlite3CacheSize())) != parameters.end())
00065 {
00066 this->setCacheSize(std::atoi((*iter).second.c_str()));
00067 }
00068 if((iter=parameters.find(Parameters::kDbSqlite3JournalMode())) != parameters.end())
00069 {
00070 this->setJournalMode(std::atoi((*iter).second.c_str()));
00071 }
00072 if((iter=parameters.find(Parameters::kDbSqlite3Synchronous())) != parameters.end())
00073 {
00074 this->setSynchronous(std::atoi((*iter).second.c_str()));
00075 }
00076 if((iter=parameters.find(Parameters::kDbSqlite3TempStore())) != parameters.end())
00077 {
00078 this->setTempStore(std::atoi((*iter).second.c_str()));
00079 }
00080 if((iter=parameters.find(Parameters::kDbSqlite3InMemory())) != parameters.end())
00081 {
00082 this->setDbInMemory(uStr2Bool((*iter).second.c_str()));
00083 }
00084 DBDriver::parseParameters(parameters);
00085 }
00086
00087 void DBDriverSqlite3::setCacheSize(unsigned int cacheSize)
00088 {
00089 if(this->isConnected())
00090 {
00091 _cacheSize = cacheSize;
00092 std::string query = "PRAGMA cache_size = ";
00093 query += uNumber2Str(_cacheSize) + ";";
00094 this->executeNoResultQuery(query.c_str());
00095 }
00096 }
00097
00098 void DBDriverSqlite3::setJournalMode(int journalMode)
00099 {
00100 if(journalMode >= 0 && journalMode < 5)
00101 {
00102 _journalMode = journalMode;
00103 if(this->isConnected())
00104 {
00105 switch(_journalMode)
00106 {
00107 case 4:
00108 this->executeNoResultQuery("PRAGMA journal_mode = OFF;");
00109 break;
00110 case 3:
00111 this->executeNoResultQuery("PRAGMA journal_mode = MEMORY;");
00112 break;
00113 case 2:
00114 this->executeNoResultQuery("PRAGMA journal_mode = PERSIST;");
00115 break;
00116 case 1:
00117 this->executeNoResultQuery("PRAGMA journal_mode = TRUNCATE;");
00118 break;
00119 case 0:
00120 default:
00121 this->executeNoResultQuery("PRAGMA journal_mode = DELETE;");
00122 break;
00123 }
00124 }
00125 }
00126 else
00127 {
00128 ULOGGER_ERROR("Wrong journal mode (%d)", journalMode);
00129 }
00130 }
00131
00132 void DBDriverSqlite3::setSynchronous(int synchronous)
00133 {
00134 if(synchronous >= 0 && synchronous < 3)
00135 {
00136 _synchronous = synchronous;
00137 if(this->isConnected())
00138 {
00139 switch(_synchronous)
00140 {
00141 case 0:
00142 this->executeNoResultQuery("PRAGMA synchronous = OFF;");
00143 break;
00144 case 1:
00145 this->executeNoResultQuery("PRAGMA synchronous = NORMAL;");
00146 break;
00147 case 2:
00148 default:
00149 this->executeNoResultQuery("PRAGMA synchronous = FULL;");
00150 break;
00151 }
00152 }
00153 }
00154 else
00155 {
00156 ULOGGER_ERROR("Wrong synchronous value (%d)", synchronous);
00157 }
00158 }
00159
00160 void DBDriverSqlite3::setTempStore(int tempStore)
00161 {
00162 if(tempStore >= 0 && tempStore < 3)
00163 {
00164 _tempStore = tempStore;
00165 if(this->isConnected())
00166 {
00167 switch(_tempStore)
00168 {
00169 case 2:
00170 this->executeNoResultQuery("PRAGMA temp_store = MEMORY;");
00171 break;
00172 case 1:
00173 this->executeNoResultQuery("PRAGMA temp_store = FILE;");
00174 break;
00175 case 0:
00176 default:
00177 this->executeNoResultQuery("PRAGMA temp_store = DEFAULT;");
00178 break;
00179 }
00180 }
00181 }
00182 else
00183 {
00184 ULOGGER_ERROR("Wrong tempStore value (%d)", tempStore);
00185 }
00186 }
00187
00188 void DBDriverSqlite3::setDbInMemory(bool dbInMemory)
00189 {
00190 if(dbInMemory != _dbInMemory)
00191 {
00192 if(this->isConnected())
00193 {
00194
00195 join(true);
00196 this->emptyTrashes();
00197 this->closeConnection();
00198 _dbInMemory = dbInMemory;
00199 this->openConnection(this->getUrl());
00200 }
00201 else
00202 {
00203 _dbInMemory = dbInMemory;
00204 }
00205 }
00206 }
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225 int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const
00226 {
00227 int rc;
00228 sqlite3 *pFile = 0;
00229 sqlite3_backup *pBackup = 0;
00230 sqlite3 *pTo = 0;
00231 sqlite3 *pFrom = 0;
00232
00233
00234
00235 rc = sqlite3_open(fileName.c_str(), &pFile);
00236 if( rc==SQLITE_OK ){
00237
00238
00239
00240
00241
00242
00243 pFrom = (isSave ? pInMemory : pFile);
00244 pTo = (isSave ? pFile : pInMemory);
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258 pBackup = sqlite3_backup_init(pTo, "main", pFrom, "main");
00259 if( pBackup ){
00260 (void)sqlite3_backup_step(pBackup, -1);
00261 (void)sqlite3_backup_finish(pBackup);
00262 }
00263 rc = sqlite3_errcode(pTo);
00264 }
00265
00266
00267
00268 (void)sqlite3_close(pFile);
00269 return rc;
00270 }
00271
00272 bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
00273 {
00274 version = "0.0.0";
00275 if(_ppDb)
00276 {
00277 UTimer timer;
00278 timer.start();
00279 int rc = SQLITE_OK;
00280 sqlite3_stmt * ppStmt = 0;
00281 std::stringstream query;
00282
00283 query << "SELECT version FROM Admin;";
00284
00285 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
00286 if(rc == SQLITE_OK)
00287 {
00288
00289 rc = sqlite3_step(ppStmt);
00290 if(rc == SQLITE_ROW)
00291 {
00292 version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
00293 rc = sqlite3_step(ppStmt);
00294 }
00295 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00296
00297
00298 rc = sqlite3_finalize(ppStmt);
00299 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00300 }
00301
00302
00303
00304
00305 return true;
00306 }
00307 return false;
00308 }
00309
00310
00311 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
00312 {
00313 this->disconnectDatabaseQuery();
00314
00315 _ppDb = 0;
00316
00317 if(url.empty())
00318 {
00319 UERROR("url is empty...");
00320 return false;
00321 }
00322
00323 int rc = SQLITE_OK;
00324 bool dbFileExist = UFile::exists(url.c_str());
00325 if(dbFileExist && overwritten)
00326 {
00327 UINFO("Deleting database %s...", url.c_str());
00328 UASSERT(UFile::erase(url.c_str()) == 0);
00329 dbFileExist = false;
00330 }
00331
00332 if(_dbInMemory)
00333 {
00334 ULOGGER_INFO("Using database \"%s\" in the memory.", url.c_str());
00335 rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
00336 }
00337 else
00338 {
00339 ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
00340 rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
00341 }
00342 if(rc != SQLITE_OK)
00343 {
00344 UFATAL("DB error : %s (path=\"%s\")", sqlite3_errmsg(_ppDb), url.c_str());
00345 _ppDb = 0;
00346 return false;
00347 }
00348
00349 if(_dbInMemory && dbFileExist)
00350 {
00351 UTimer timer;
00352 timer.start();
00353 ULOGGER_DEBUG("Loading DB ...");
00354 rc = loadOrSaveDb(_ppDb, url, 0);
00355 ULOGGER_INFO("Loading DB time = %fs, (%s)", timer.ticks(), url.c_str());
00356 if(rc != SQLITE_OK)
00357 {
00358 UFATAL("DB error 2 : %s", sqlite3_errmsg(_ppDb));
00359 sqlite3_close(_ppDb);
00360 _ppDb = 0;
00361 return false;
00362 }
00363 }
00364
00365 if(!dbFileExist)
00366 {
00367 ULOGGER_INFO("Database \"%s\" doesn't exist, creating a new one...", url.c_str());
00368
00369 std::string schema = DATABASESCHEMA_SQL;
00370 schema = uHex2Str(schema);
00371 this->executeNoResultQuery(schema.c_str());
00372 }
00373 UASSERT(this->getDatabaseVersionQuery(_version));
00374 UINFO("Database version = %s", _version.c_str());
00375
00376 if(uStrNumCmp(_version, RTABMAP_VERSION) > 0)
00377 {
00378 UERROR("Opened database version (%s) is more recent than rtabmap "
00379 "installed version (%s). Please update rtabmap to new version!",
00380 _version.c_str(), RTABMAP_VERSION);
00381 this->disconnectDatabaseQuery(false);
00382 return false;
00383 }
00384
00385
00386 this->setCacheSize(_cacheSize);
00387 this->setJournalMode(_journalMode);
00388 this->setSynchronous(_synchronous);
00389 this->setTempStore(_tempStore);
00390
00391 return true;
00392 }
00393 void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
00394 {
00395 UDEBUG("");
00396 if(_ppDb)
00397 {
00398 int rc = SQLITE_OK;
00399
00400 sqlite3_stmt * pStmt;
00401 while( (pStmt = sqlite3_next_stmt(_ppDb, 0))!=0 )
00402 {
00403 rc = sqlite3_finalize(pStmt);
00404 if(rc != SQLITE_OK)
00405 {
00406 UERROR("");
00407 }
00408 }
00409
00410 if(save && _dbInMemory)
00411 {
00412 UTimer timer;
00413 timer.start();
00414 UINFO("Saving database to %s ...", this->getUrl().c_str());
00415 rc = loadOrSaveDb(_ppDb, this->getUrl(), 1);
00416 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00417 ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
00418 }
00419
00420
00421 UINFO("Disconnecting database %s...", this->getUrl().c_str());
00422 sqlite3_close(_ppDb);
00423 _ppDb = 0;
00424 }
00425 }
00426
00427 bool DBDriverSqlite3::isConnectedQuery() const
00428 {
00429 return _ppDb != 0;
00430 }
00431
00432
00433 void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
00434 {
00435 if(_ppDb)
00436 {
00437 UTimer timer;
00438 timer.start();
00439 int rc;
00440 rc = sqlite3_exec(_ppDb, sql.c_str(), 0, 0, 0);
00441 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s, the query is %s", sqlite3_errmsg(_ppDb), sql.c_str()).c_str());
00442 UDEBUG("Time=%fs", timer.ticks());
00443 }
00444 }
00445
00446 long DBDriverSqlite3::getMemoryUsedQuery() const
00447 {
00448
00449
00450 return sqlite3_memory_used();
00451
00452
00453
00454
00455
00456 }
00457
00458 long DBDriverSqlite3::getImagesMemoryUsedQuery() const
00459 {
00460 UDEBUG("");
00461 long size = 0L;
00462 if(_ppDb)
00463 {
00464 std::string query;
00465 if(uStrNumCmp(_version, "0.10.0") >= 0)
00466 {
00467 query = "SELECT sum(length(image)) from Data;";
00468 }
00469 else
00470 {
00471 query = "SELECT sum(length(data)) from Image;";
00472 }
00473
00474 int rc = SQLITE_OK;
00475 sqlite3_stmt * ppStmt = 0;
00476 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00477 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00478 rc = sqlite3_step(ppStmt);
00479 if(rc == SQLITE_ROW)
00480 {
00481 size = sqlite3_column_int64(ppStmt, 0);
00482 rc = sqlite3_step(ppStmt);
00483 }
00484 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00485 rc = sqlite3_finalize(ppStmt);
00486 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00487 }
00488 return size;
00489 }
00490 long DBDriverSqlite3::getDepthImagesMemoryUsedQuery() const
00491 {
00492 UDEBUG("");
00493 long size = 0L;
00494 if(_ppDb)
00495 {
00496 std::string query;
00497 if(uStrNumCmp(_version, "0.10.0") >= 0)
00498 {
00499 query = "SELECT sum(length(depth)) from Data;";
00500 }
00501 else
00502 {
00503 query = "SELECT sum(length(data)) from Depth;";
00504 }
00505
00506 int rc = SQLITE_OK;
00507 sqlite3_stmt * ppStmt = 0;
00508 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00509 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00510 rc = sqlite3_step(ppStmt);
00511 if(rc == SQLITE_ROW)
00512 {
00513 size = sqlite3_column_int64(ppStmt, 0);
00514 rc = sqlite3_step(ppStmt);
00515 }
00516 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00517 rc = sqlite3_finalize(ppStmt);
00518 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00519 }
00520 return size;
00521 }
00522 long DBDriverSqlite3::getLaserScansMemoryUsedQuery() const
00523 {
00524 UDEBUG("");
00525 long size = 0L;
00526 if(_ppDb)
00527 {
00528 std::string query;
00529 if(uStrNumCmp(_version, "0.10.0") >= 0)
00530 {
00531 query = "SELECT sum(length(scan)) from Data;";
00532 }
00533 else
00534 {
00535 query = "SELECT sum(length(data2d)) from Depth;";
00536 }
00537
00538 int rc = SQLITE_OK;
00539 sqlite3_stmt * ppStmt = 0;
00540 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00541 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00542 rc = sqlite3_step(ppStmt);
00543 if(rc == SQLITE_ROW)
00544 {
00545 size = sqlite3_column_int64(ppStmt, 0);
00546 rc = sqlite3_step(ppStmt);
00547 }
00548 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00549 rc = sqlite3_finalize(ppStmt);
00550 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00551 }
00552 return size;
00553 }
00554 long DBDriverSqlite3::getUserDataMemoryUsedQuery() const
00555 {
00556 UDEBUG("");
00557 long size = 0L;
00558 if(_ppDb)
00559 {
00560 std::string query;
00561 if(uStrNumCmp(_version, "0.10.1") >= 0)
00562 {
00563 query = "SELECT sum(length(user_data)) from Data;";
00564 }
00565 else if(uStrNumCmp(_version, "0.8.8") >= 0)
00566 {
00567 query = "SELECT sum(length(user_data)) from Node;";
00568 }
00569 else
00570 {
00571 return size;
00572 }
00573
00574 int rc = SQLITE_OK;
00575 sqlite3_stmt * ppStmt = 0;
00576 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00577 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00578 rc = sqlite3_step(ppStmt);
00579 if(rc == SQLITE_ROW)
00580 {
00581 size = sqlite3_column_int64(ppStmt, 0);
00582 rc = sqlite3_step(ppStmt);
00583 }
00584 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00585 rc = sqlite3_finalize(ppStmt);
00586 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00587 }
00588 return size;
00589 }
00590 long DBDriverSqlite3::getWordsMemoryUsedQuery() const
00591 {
00592 UDEBUG("");
00593 long size = 0L;
00594 if(_ppDb)
00595 {
00596 std::string query = "SELECT sum(length(descriptor)) from Word;";
00597
00598 int rc = SQLITE_OK;
00599 sqlite3_stmt * ppStmt = 0;
00600 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00601 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00602 rc = sqlite3_step(ppStmt);
00603 if(rc == SQLITE_ROW)
00604 {
00605 size = sqlite3_column_int64(ppStmt, 0);
00606 rc = sqlite3_step(ppStmt);
00607 }
00608 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00609 rc = sqlite3_finalize(ppStmt);
00610 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00611 }
00612 return size;
00613 }
00614 int DBDriverSqlite3::getLastNodesSizeQuery() const
00615 {
00616 UDEBUG("");
00617 int size = 0;
00618 if(_ppDb)
00619 {
00620 std::string query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
00621
00622 int rc = SQLITE_OK;
00623 sqlite3_stmt * ppStmt = 0;
00624 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00625 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00626 rc = sqlite3_step(ppStmt);
00627 if(rc == SQLITE_ROW)
00628 {
00629 size = sqlite3_column_int(ppStmt, 0);
00630 rc = sqlite3_step(ppStmt);
00631 }
00632 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00633 rc = sqlite3_finalize(ppStmt);
00634 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00635 }
00636 return size;
00637 }
00638 int DBDriverSqlite3::getLastDictionarySizeQuery() const
00639 {
00640 UDEBUG("");
00641 int size = 0;
00642 if(_ppDb)
00643 {
00644 std::string query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
00645
00646 int rc = SQLITE_OK;
00647 sqlite3_stmt * ppStmt = 0;
00648 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00649 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00650 rc = sqlite3_step(ppStmt);
00651 if(rc == SQLITE_ROW)
00652 {
00653 size = sqlite3_column_int(ppStmt, 0);
00654 rc = sqlite3_step(ppStmt);
00655 }
00656 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00657 rc = sqlite3_finalize(ppStmt);
00658 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00659 }
00660 return size;
00661 }
00662 int DBDriverSqlite3::getTotalNodesSizeQuery() const
00663 {
00664 UDEBUG("");
00665 int size = 0;
00666 if(_ppDb)
00667 {
00668 std::string query = "SELECT count(id) from Node;";
00669
00670 int rc = SQLITE_OK;
00671 sqlite3_stmt * ppStmt = 0;
00672 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00673 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00674 rc = sqlite3_step(ppStmt);
00675 if(rc == SQLITE_ROW)
00676 {
00677 size = sqlite3_column_int(ppStmt, 0);
00678 rc = sqlite3_step(ppStmt);
00679 }
00680 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00681 rc = sqlite3_finalize(ppStmt);
00682 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00683 }
00684 return size;
00685 }
00686 int DBDriverSqlite3::getTotalDictionarySizeQuery() const
00687 {
00688 UDEBUG("");
00689 int size = 0;
00690 if(_ppDb)
00691 {
00692 std::string query = "SELECT count(id) from Word;";
00693
00694 int rc = SQLITE_OK;
00695 sqlite3_stmt * ppStmt = 0;
00696 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00697 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00698 rc = sqlite3_step(ppStmt);
00699 if(rc == SQLITE_ROW)
00700 {
00701 size = sqlite3_column_int(ppStmt, 0);
00702 rc = sqlite3_step(ppStmt);
00703 }
00704 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00705 rc = sqlite3_finalize(ppStmt);
00706 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00707 }
00708 return size;
00709 }
00710
00711 ParametersMap DBDriverSqlite3::getLastParametersQuery() const
00712 {
00713 UDEBUG("");
00714 ParametersMap parameters;
00715 if(_ppDb)
00716 {
00717 if(uStrNumCmp(_version, "0.11.8") >= 0)
00718 {
00719 std::string query = "SELECT parameters "
00720 "FROM Statistics "
00721 "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
00722
00723 int rc = SQLITE_OK;
00724 sqlite3_stmt * ppStmt = 0;
00725 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00726 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00727 rc = sqlite3_step(ppStmt);
00728 if(rc == SQLITE_ROW)
00729 {
00730 std::string text((const char *)sqlite3_column_text(ppStmt, 0));
00731
00732 if(text.size())
00733 {
00734 parameters = Parameters::deserialize(text);
00735 }
00736
00737 rc = sqlite3_step(ppStmt);
00738 }
00739 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00740 rc = sqlite3_finalize(ppStmt);
00741 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00742 }
00743 }
00744 return parameters;
00745 }
00746
00747 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
00748 {
00749 UDEBUG("load data for %d signatures", (int)signatures.size());
00750 if(_ppDb)
00751 {
00752 UTimer timer;
00753 timer.start();
00754 int rc = SQLITE_OK;
00755 sqlite3_stmt * ppStmt = 0;
00756 std::stringstream query;
00757
00758 if(uStrNumCmp(_version, "0.10.7") >= 0)
00759 {
00760 query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
00761 << "FROM Data "
00762 << "WHERE id = ?"
00763 <<";";
00764 }
00765 else if(uStrNumCmp(_version, "0.10.1") >= 0)
00766 {
00767 query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
00768 << "FROM Data "
00769 << "WHERE id = ?"
00770 <<";";
00771 }
00772 else if(uStrNumCmp(_version, "0.10.0") >= 0)
00773 {
00774 query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data "
00775 << "FROM Data "
00776 << "INNER JOIN Node "
00777 << "ON Data.id = Node.id "
00778 << "WHERE Data.id = ?"
00779 <<";";
00780 }
00781 else if(uStrNumCmp(_version, "0.8.11") >= 0)
00782 {
00783 query << "SELECT Image.data, "
00784 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data "
00785 << "FROM Image "
00786 << "INNER JOIN Node "
00787 << "on Image.id = Node.id "
00788 << "LEFT OUTER JOIN Depth "
00789 << "ON Image.id = Depth.id "
00790 << "WHERE Image.id = ?"
00791 <<";";
00792 }
00793 else if(uStrNumCmp(_version, "0.8.8") >= 0)
00794 {
00795 query << "SELECT Image.data, "
00796 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data "
00797 << "FROM Image "
00798 << "INNER JOIN Node "
00799 << "on Image.id = Node.id "
00800 << "LEFT OUTER JOIN Depth "
00801 << "ON Image.id = Depth.id "
00802 << "WHERE Image.id = ?"
00803 <<";";
00804 }
00805 else if(uStrNumCmp(_version, "0.7.0") >= 0)
00806 {
00807 query << "SELECT Image.data, "
00808 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
00809 << "FROM Image "
00810 << "LEFT OUTER JOIN Depth "
00811 << "ON Image.id = Depth.id "
00812 << "WHERE Image.id = ?"
00813 <<";";
00814 }
00815 else
00816 {
00817 query << "SELECT Image.data, "
00818 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
00819 << "FROM Image "
00820 << "LEFT OUTER JOIN Depth "
00821 << "ON Image.id = Depth.id "
00822 << "WHERE Image.id = ?"
00823 <<";";
00824 }
00825
00826 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
00827 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00828
00829 const void * data = 0;
00830 int dataSize = 0;
00831 int index = 0;
00832
00833 for(std::list<Signature*>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
00834 {
00835 UASSERT(*iter != 0);
00836
00837 ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
00838
00839 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
00840 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00841
00842
00843 rc = sqlite3_step(ppStmt);
00844 if(rc == SQLITE_ROW)
00845 {
00846 index = 0;
00847
00848 cv::Mat imageCompressed;
00849 cv::Mat depthOrRightCompressed;
00850 std::vector<CameraModel> models;
00851 StereoCameraModel stereoModel;
00852 Transform localTransform = Transform::getIdentity();
00853 cv::Mat scanCompressed;
00854 cv::Mat userDataCompressed;
00855
00856 data = sqlite3_column_blob(ppStmt, index);
00857 dataSize = sqlite3_column_bytes(ppStmt, index++);
00858
00859
00860 if(dataSize>4 && data)
00861 {
00862 imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
00863 }
00864
00865 data = sqlite3_column_blob(ppStmt, index);
00866 dataSize = sqlite3_column_bytes(ppStmt, index++);
00867
00868
00869 if(dataSize>4 && data)
00870 {
00871 depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
00872 }
00873
00874 if(uStrNumCmp(_version, "0.10.0") < 0)
00875 {
00876 data = sqlite3_column_blob(ppStmt, index);
00877 dataSize = sqlite3_column_bytes(ppStmt, index++);
00878 if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
00879 {
00880 memcpy(localTransform.data(), data, dataSize);
00881 }
00882 }
00883
00884
00885 if(uStrNumCmp(_version, "0.10.0") >= 0)
00886 {
00887 data = sqlite3_column_blob(ppStmt, index);
00888 dataSize = sqlite3_column_bytes(ppStmt, index++);
00889
00890
00891 if(dataSize > 0 && data)
00892 {
00893 float * dataFloat = (float*)data;
00894 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
00895 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
00896 {
00897 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
00898 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
00899 int max = cameraCount*(6+localTransform.size());
00900 for(int i=0; i<max; i+=6+localTransform.size())
00901 {
00902
00903 localTransform = Transform::getIdentity();
00904 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
00905 models.push_back(CameraModel(
00906 (double)dataFloat[i],
00907 (double)dataFloat[i+1],
00908 (double)dataFloat[i+2],
00909 (double)dataFloat[i+3],
00910 localTransform));
00911 models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
00912 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
00913 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
00914 localTransform.prettyPrint().c_str());
00915 }
00916 }
00917 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
00918 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
00919 {
00920 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
00921 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
00922 int max = cameraCount*(4+localTransform.size());
00923 for(int i=0; i<max; i+=4+localTransform.size())
00924 {
00925
00926 localTransform = Transform::getIdentity();
00927 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
00928 models.push_back(CameraModel(
00929 (double)dataFloat[i],
00930 (double)dataFloat[i+1],
00931 (double)dataFloat[i+2],
00932 (double)dataFloat[i+3],
00933 localTransform));
00934 }
00935 }
00936 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
00937 {
00938 UDEBUG("Loading calibration of a stereo camera");
00939 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
00940 stereoModel = StereoCameraModel(
00941 dataFloat[0],
00942 dataFloat[1],
00943 dataFloat[2],
00944 dataFloat[3],
00945 dataFloat[4],
00946 localTransform);
00947 }
00948 else
00949 {
00950 UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
00951 }
00952 }
00953
00954 }
00955 else if(uStrNumCmp(_version, "0.7.0") >= 0)
00956 {
00957 double fx = sqlite3_column_double(ppStmt, index++);
00958 double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
00959 double cx = sqlite3_column_double(ppStmt, index++);
00960 double cy = sqlite3_column_double(ppStmt, index++);
00961 if(fyOrBaseline < 1.0)
00962 {
00963
00964 stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
00965 }
00966 else
00967 {
00968 models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
00969 }
00970 }
00971 else
00972 {
00973 float depthConstant = sqlite3_column_double(ppStmt, index++);
00974 float fx = 1.0f/depthConstant;
00975 float fy = 1.0f/depthConstant;
00976 float cx = 0.0f;
00977 float cy = 0.0f;
00978 models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
00979 }
00980
00981 int laserScanMaxPts = 0;
00982 if(uStrNumCmp(_version, "0.8.11") >= 0)
00983 {
00984 laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
00985 }
00986
00987 float laserScanMaxRange = 0.0f;
00988 if(uStrNumCmp(_version, "0.10.7") >= 0)
00989 {
00990 laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
00991 }
00992
00993 data = sqlite3_column_blob(ppStmt, index);
00994 dataSize = sqlite3_column_bytes(ppStmt, index++);
00995
00996 if(dataSize>4 && data)
00997 {
00998 scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
00999 }
01000
01001 if(uStrNumCmp(_version, "0.8.8") >= 0)
01002 {
01003 data = sqlite3_column_blob(ppStmt, index);
01004 dataSize = sqlite3_column_bytes(ppStmt, index++);
01005
01006 if(dataSize>4 && data)
01007 {
01008 if(uStrNumCmp(_version, "0.10.1") >= 0)
01009 {
01010 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01011 }
01012 else
01013 {
01014
01015 userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
01016 }
01017 }
01018 }
01019
01020 if(models.size())
01021 {
01022 (*iter)->sensorData() = SensorData(
01023 scanCompressed,
01024 laserScanMaxPts,
01025 laserScanMaxRange,
01026 imageCompressed,
01027 depthOrRightCompressed,
01028 models,
01029 (*iter)->id(),
01030 0,
01031 userDataCompressed);
01032 }
01033 else
01034 {
01035 (*iter)->sensorData() = SensorData(
01036 scanCompressed,
01037 laserScanMaxPts,
01038 laserScanMaxRange,
01039 imageCompressed,
01040 depthOrRightCompressed,
01041 stereoModel,
01042 (*iter)->id(),
01043 0,
01044 userDataCompressed);
01045 }
01046
01047 rc = sqlite3_step(ppStmt);
01048 }
01049 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01050
01051
01052 rc = sqlite3_reset(ppStmt);
01053 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01054 }
01055
01056
01057 rc = sqlite3_finalize(ppStmt);
01058 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01059 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01060 }
01061 }
01062
01063 bool DBDriverSqlite3::getCalibrationQuery(
01064 int signatureId,
01065 std::vector<CameraModel> & models,
01066 StereoCameraModel & stereoModel) const
01067 {
01068 bool found = false;
01069 if(_ppDb && signatureId)
01070 {
01071 int rc = SQLITE_OK;
01072 sqlite3_stmt * ppStmt = 0;
01073 std::stringstream query;
01074
01075 if(uStrNumCmp(_version, "0.10.0") >= 0)
01076 {
01077 query << "SELECT calibration "
01078 << "FROM Data "
01079 << "WHERE id = " << signatureId
01080 <<";";
01081 }
01082 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01083 {
01084 query << "SELECT local_transform, fx, fy, cx, cy "
01085 << "FROM Depth "
01086 << "WHERE id = " << signatureId
01087 <<";";
01088 }
01089 else
01090 {
01091 query << "SELECT local_transform, constant "
01092 << "FROM Depth "
01093 << "WHERE id = " << signatureId
01094 <<";";
01095 }
01096
01097 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01098 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01099
01100 const void * data = 0;
01101 int dataSize = 0;
01102 Transform localTransform;
01103
01104
01105 rc = sqlite3_step(ppStmt);
01106 if(rc == SQLITE_ROW)
01107 {
01108 found = true;
01109 int index = 0;
01110
01111
01112 if(uStrNumCmp(_version, "0.10.0") >= 0)
01113 {
01114 data = sqlite3_column_blob(ppStmt, index);
01115 dataSize = sqlite3_column_bytes(ppStmt, index++);
01116
01117
01118 if(dataSize > 0 && data)
01119 {
01120 float * dataFloat = (float*)data;
01121 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
01122 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
01123 {
01124 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
01125 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01126 int max = cameraCount*(6+localTransform.size());
01127 for(int i=0; i<max; i+=6+localTransform.size())
01128 {
01129
01130 localTransform = Transform::getIdentity();
01131 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
01132 models.push_back(CameraModel(
01133 (double)dataFloat[i],
01134 (double)dataFloat[i+1],
01135 (double)dataFloat[i+2],
01136 (double)dataFloat[i+3],
01137 localTransform));
01138 models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
01139 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
01140 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
01141 localTransform.prettyPrint().c_str());
01142 }
01143 }
01144 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
01145 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
01146 {
01147 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
01148 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01149 int max = cameraCount*(4+localTransform.size());
01150 for(int i=0; i<max; i+=4+localTransform.size())
01151 {
01152
01153 localTransform = Transform::getIdentity();
01154 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
01155 models.push_back(CameraModel(
01156 (double)dataFloat[i],
01157 (double)dataFloat[i+1],
01158 (double)dataFloat[i+2],
01159 (double)dataFloat[i+3],
01160 localTransform));
01161 }
01162 }
01163 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
01164 {
01165 UDEBUG("Loading calibration of a stereo camera");
01166 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
01167 stereoModel = StereoCameraModel(
01168 dataFloat[0],
01169 dataFloat[1],
01170 dataFloat[2],
01171 dataFloat[3],
01172 dataFloat[4],
01173 localTransform);
01174 }
01175 else
01176 {
01177 UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
01178 }
01179 }
01180
01181 }
01182 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01183 {
01184 double fx = sqlite3_column_double(ppStmt, index++);
01185 double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
01186 double cx = sqlite3_column_double(ppStmt, index++);
01187 double cy = sqlite3_column_double(ppStmt, index++);
01188 if(fyOrBaseline < 1.0)
01189 {
01190
01191 stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
01192 }
01193 else
01194 {
01195 models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
01196 }
01197 }
01198 else
01199 {
01200 float depthConstant = sqlite3_column_double(ppStmt, index++);
01201 float fx = 1.0f/depthConstant;
01202 float fy = 1.0f/depthConstant;
01203 float cx = 0.0f;
01204 float cy = 0.0f;
01205 models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
01206 }
01207
01208 rc = sqlite3_step(ppStmt);
01209 }
01210 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01211
01212
01213 rc = sqlite3_finalize(ppStmt);
01214 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01215 }
01216 return found;
01217 }
01218
01219 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
01220 Transform & pose,
01221 int & mapId,
01222 int & weight,
01223 std::string & label,
01224 double & stamp,
01225 Transform & groundTruthPose) const
01226 {
01227 bool found = false;
01228 if(_ppDb && signatureId)
01229 {
01230 int rc = SQLITE_OK;
01231 sqlite3_stmt * ppStmt = 0;
01232 std::stringstream query;
01233
01234 if(uStrNumCmp(_version, "0.11.1") >= 0)
01235 {
01236 query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
01237 "FROM Node "
01238 "WHERE id = " << signatureId <<
01239 ";";
01240 }
01241 else if(uStrNumCmp(_version, "0.8.5") >= 0)
01242 {
01243 query << "SELECT pose, map_id, weight, label, stamp "
01244 "FROM Node "
01245 "WHERE id = " << signatureId <<
01246 ";";
01247 }
01248 else
01249 {
01250 query << "SELECT pose, map_id, weight "
01251 "FROM Node "
01252 "WHERE id = " << signatureId <<
01253 ";";
01254 }
01255
01256 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01257 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01258
01259 const void * data = 0;
01260 int dataSize = 0;
01261
01262
01263 rc = sqlite3_step(ppStmt);
01264 if(rc == SQLITE_ROW)
01265 {
01266 found = true;
01267 int index = 0;
01268 data = sqlite3_column_blob(ppStmt, index);
01269 dataSize = sqlite3_column_bytes(ppStmt, index++);
01270 if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
01271 {
01272 memcpy(pose.data(), data, dataSize);
01273 }
01274
01275 mapId = sqlite3_column_int(ppStmt, index++);
01276 weight = sqlite3_column_int(ppStmt, index++);
01277
01278 if(uStrNumCmp(_version, "0.8.5") >= 0)
01279 {
01280 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
01281 if(p)
01282 {
01283 label = reinterpret_cast<const char*>(p);
01284 }
01285 stamp = sqlite3_column_double(ppStmt, index++);
01286 }
01287
01288 if(uStrNumCmp(_version, "0.11.1") >= 0)
01289 {
01290 data = sqlite3_column_blob(ppStmt, index);
01291 dataSize = sqlite3_column_bytes(ppStmt, index++);
01292 if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
01293 {
01294 memcpy(groundTruthPose.data(), data, dataSize);
01295 }
01296 }
01297
01298 rc = sqlite3_step(ppStmt);
01299 }
01300 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01301
01302
01303 rc = sqlite3_finalize(ppStmt);
01304 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01305 }
01306 return found;
01307 }
01308
01309
01310 void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
01311 {
01312 if(_ppDb)
01313 {
01314 UTimer timer;
01315 timer.start();
01316 int rc = SQLITE_OK;
01317 sqlite3_stmt * ppStmt = 0;
01318 std::stringstream query;
01319
01320 query << "SELECT DISTINCT id "
01321 << "FROM Node ";
01322 if(ignoreChildren)
01323 {
01324 query << "INNER JOIN Link "
01325 << "ON id = to_id ";
01326 }
01327 if(ignoreBadSignatures)
01328 {
01329 query << "WHERE id in (select node_id from Map_Node_Word) ";
01330 }
01331 query << "ORDER BY id";
01332
01333 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01334 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01335
01336
01337
01338 rc = sqlite3_step(ppStmt);
01339 while(rc == SQLITE_ROW)
01340 {
01341 ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0));
01342 rc = sqlite3_step(ppStmt);
01343 }
01344 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01345
01346
01347 rc = sqlite3_finalize(ppStmt);
01348 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01349 ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
01350 }
01351 }
01352
01353 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const
01354 {
01355 links.clear();
01356 if(_ppDb)
01357 {
01358 UTimer timer;
01359 timer.start();
01360 int rc = SQLITE_OK;
01361 sqlite3_stmt * ppStmt = 0;
01362 std::stringstream query;
01363
01364 if(uStrNumCmp(_version, "0.10.10") >= 0)
01365 {
01366 query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
01367 }
01368 else if(uStrNumCmp(_version, "0.8.4") >= 0)
01369 {
01370 query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
01371 }
01372 else if(uStrNumCmp(_version, "0.7.4") >= 0)
01373 {
01374 query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
01375 }
01376 else
01377 {
01378 query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
01379 }
01380
01381 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01382 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01383
01384 int fromId = -1;
01385 int toId = -1;
01386 int type = Link::kUndef;
01387 float rotVariance = 1.0f;
01388 float transVariance = 1.0f;
01389 const void * data = 0;
01390 int dataSize = 0;
01391
01392
01393 rc = sqlite3_step(ppStmt);
01394 while(rc == SQLITE_ROW)
01395 {
01396 int index = 0;
01397
01398 fromId = sqlite3_column_int(ppStmt, index++);
01399 toId = sqlite3_column_int(ppStmt, index++);
01400 type = sqlite3_column_int(ppStmt, index++);
01401
01402 data = sqlite3_column_blob(ppStmt, index);
01403 dataSize = sqlite3_column_bytes(ppStmt, index++);
01404
01405 Transform transform;
01406 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
01407 {
01408 memcpy(transform.data(), data, dataSize);
01409 }
01410 else if(dataSize)
01411 {
01412 UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
01413 }
01414
01415 if(!ignoreNullLinks || !transform.isNull())
01416 {
01417 if(uStrNumCmp(_version, "0.8.4") >= 0)
01418 {
01419 rotVariance = sqlite3_column_double(ppStmt, index++);
01420 transVariance = sqlite3_column_double(ppStmt, index++);
01421
01422 cv::Mat userDataCompressed;
01423 if(uStrNumCmp(_version, "0.10.10") >= 0)
01424 {
01425 const void * data = sqlite3_column_blob(ppStmt, index);
01426 dataSize = sqlite3_column_bytes(ppStmt, index++);
01427
01428 if(dataSize>4 && data)
01429 {
01430 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01431 }
01432 }
01433
01434 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
01435 }
01436 else if(uStrNumCmp(_version, "0.7.4") >= 0)
01437 {
01438 rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
01439 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
01440 }
01441 else
01442 {
01443
01444 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
01445 }
01446 }
01447
01448 rc = sqlite3_step(ppStmt);
01449 }
01450
01451 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01452
01453
01454 rc = sqlite3_finalize(ppStmt);
01455 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01456 }
01457 }
01458
01459 void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id) const
01460 {
01461 if(_ppDb)
01462 {
01463 UDEBUG("get last id from table \"%s\"", tableName.c_str());
01464 UTimer timer;
01465 timer.start();
01466 int rc = SQLITE_OK;
01467 sqlite3_stmt * ppStmt = 0;
01468 std::stringstream query;
01469
01470 query << "SELECT max(id) "
01471 << "FROM " << tableName
01472 << ";";
01473
01474 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01475 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01476
01477
01478
01479 rc = sqlite3_step(ppStmt);
01480 if(rc == SQLITE_ROW)
01481 {
01482 id = sqlite3_column_int(ppStmt, 0);
01483 rc = sqlite3_step(ppStmt);
01484 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01485 }
01486 else
01487 {
01488 ULOGGER_ERROR("No result !?! from the DB");
01489 }
01490
01491
01492 rc = sqlite3_finalize(ppStmt);
01493 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01494 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01495 }
01496 }
01497
01498 void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
01499 {
01500 ni = 0;
01501 if(_ppDb)
01502 {
01503 UTimer timer;
01504 timer.start();
01505 int rc = SQLITE_OK;
01506 sqlite3_stmt * ppStmt = 0;
01507 std::stringstream query;
01508
01509
01510 query << "SELECT count(word_id) "
01511 << "FROM Map_Node_Word "
01512 << "WHERE node_id=" << nodeId << ";";
01513
01514
01515
01516
01517
01518 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01519 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01520
01521
01522
01523 rc = sqlite3_step(ppStmt);
01524 if(rc == SQLITE_ROW)
01525 {
01526 ni = sqlite3_column_int(ppStmt, 0);
01527 rc = sqlite3_step(ppStmt);
01528 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01529 }
01530 else
01531 {
01532 ULOGGER_ERROR("No result !?! from the DB, node=%d",nodeId);
01533 }
01534
01535
01536
01537 rc = sqlite3_finalize(ppStmt);
01538 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01539 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01540 }
01541 }
01542
01543 void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
01544 {
01545 if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
01546 {
01547 UTimer timer;
01548 timer.start();
01549 int rc = SQLITE_OK;
01550 sqlite3_stmt * ppStmt = 0;
01551 std::stringstream query;
01552 query << "SELECT id FROM Node WHERE label='" << label <<"'";
01553
01554 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01555 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01556
01557
01558 rc = sqlite3_step(ppStmt);
01559 if(rc == SQLITE_ROW)
01560 {
01561 id = sqlite3_column_int(ppStmt, 0);
01562 rc = sqlite3_step(ppStmt);
01563 }
01564 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01565
01566
01567 rc = sqlite3_finalize(ppStmt);
01568 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01569 ULOGGER_DEBUG("Time=%f", timer.ticks());
01570 }
01571 }
01572
01573 void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
01574 {
01575 if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
01576 {
01577 UTimer timer;
01578 timer.start();
01579 int rc = SQLITE_OK;
01580 sqlite3_stmt * ppStmt = 0;
01581 std::stringstream query;
01582 query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
01583
01584 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01585 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01586
01587
01588 rc = sqlite3_step(ppStmt);
01589 while(rc == SQLITE_ROW)
01590 {
01591 int index = 0;
01592 int id = sqlite3_column_int(ppStmt, index++);
01593 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
01594 if(p)
01595 {
01596 std::string label = reinterpret_cast<const char*>(p);
01597 if(!label.empty())
01598 {
01599 labels.insert(std::make_pair(id, label));
01600 }
01601 }
01602 rc = sqlite3_step(ppStmt);
01603 }
01604 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01605
01606
01607 rc = sqlite3_finalize(ppStmt);
01608 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01609 ULOGGER_DEBUG("Time=%f", timer.ticks());
01610 }
01611 }
01612
01613 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
01614 {
01615 weight = 0;
01616 if(_ppDb)
01617 {
01618 UTimer timer;
01619 timer.start();
01620 int rc = SQLITE_OK;
01621 sqlite3_stmt * ppStmt = 0;
01622 std::stringstream query;
01623
01624 query << "SELECT weight FROM node WHERE id = "
01625 << nodeId
01626 << ";";
01627
01628 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01629 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01630
01631
01632
01633 rc = sqlite3_step(ppStmt);
01634 if(rc == SQLITE_ROW)
01635 {
01636 weight= sqlite3_column_int(ppStmt, 0);
01637 rc = sqlite3_step(ppStmt);
01638 }
01639 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01640
01641
01642 rc = sqlite3_finalize(ppStmt);
01643 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01644 }
01645 }
01646
01647
01648 void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
01649 {
01650 ULOGGER_DEBUG("count=%d", (int)ids.size());
01651 if(_ppDb && ids.size())
01652 {
01653 std::string type;
01654 UTimer timer;
01655 timer.start();
01656 int rc = SQLITE_OK;
01657 sqlite3_stmt * ppStmt = 0;
01658 std::stringstream query;
01659 unsigned int loaded = 0;
01660
01661
01662 if(uStrNumCmp(_version, "0.11.1") >= 0)
01663 {
01664 query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
01665 << "FROM Node "
01666 << "WHERE id=?;";
01667 }
01668 else if(uStrNumCmp(_version, "0.8.5") >= 0)
01669 {
01670 query << "SELECT id, map_id, weight, pose, stamp, label "
01671 << "FROM Node "
01672 << "WHERE id=?;";
01673 }
01674 else
01675 {
01676 query << "SELECT id, map_id, weight, pose "
01677 << "FROM Node "
01678 << "WHERE id=?;";
01679 }
01680
01681 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01682 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01683
01684 for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
01685 {
01686
01687
01688 rc = sqlite3_bind_int(ppStmt, 1, *iter);
01689 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01690
01691 int id = 0;
01692 int mapId = 0;
01693 double stamp = 0.0;
01694 int weight = 0;
01695 Transform pose;
01696 Transform groundTruthPose;
01697 const void * data = 0;
01698 int dataSize = 0;
01699 std::string label;
01700
01701
01702 rc = sqlite3_step(ppStmt);
01703 if(rc == SQLITE_ROW)
01704 {
01705 int index = 0;
01706 id = sqlite3_column_int(ppStmt, index++);
01707 mapId = sqlite3_column_int(ppStmt, index++);
01708 weight = sqlite3_column_int(ppStmt, index++);
01709
01710 data = sqlite3_column_blob(ppStmt, index);
01711 dataSize = sqlite3_column_bytes(ppStmt, index++);
01712 if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
01713 {
01714 memcpy(pose.data(), data, dataSize);
01715 }
01716
01717 if(uStrNumCmp(_version, "0.8.5") >= 0)
01718 {
01719 stamp = sqlite3_column_double(ppStmt, index++);
01720 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
01721 if(p)
01722 {
01723 label = reinterpret_cast<const char*>(p);
01724 }
01725 }
01726
01727 if(uStrNumCmp(_version, "0.11.1") >= 0)
01728 {
01729 data = sqlite3_column_blob(ppStmt, index);
01730 dataSize = sqlite3_column_bytes(ppStmt, index++);
01731 if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
01732 {
01733 memcpy(groundTruthPose.data(), data, dataSize);
01734 }
01735 }
01736
01737 rc = sqlite3_step(ppStmt);
01738 }
01739 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01740
01741
01742 if(id)
01743 {
01744 ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
01745 Signature * s = new Signature(
01746 id,
01747 mapId,
01748 weight,
01749 stamp,
01750 label,
01751 pose,
01752 groundTruthPose);
01753 s->setSaved(true);
01754 nodes.push_back(s);
01755 ++loaded;
01756 }
01757 else
01758 {
01759 UERROR("Signature %d not found in database!", *iter);
01760 }
01761
01762
01763 rc = sqlite3_reset(ppStmt);
01764 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01765 }
01766
01767
01768 rc = sqlite3_finalize(ppStmt);
01769 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01770
01771 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01772
01773
01774 std::stringstream query2;
01775 if(uStrNumCmp(_version, "0.11.2") >= 0)
01776 {
01777 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
01778 "FROM Map_Node_Word "
01779 "WHERE node_id = ? ";
01780 }
01781 else
01782 {
01783 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
01784 "FROM Map_Node_Word "
01785 "WHERE node_id = ? ";
01786 }
01787
01788 query2 << " ORDER BY word_id";
01789 query2 << ";";
01790
01791 rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
01792 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01793
01794 std::set<int> calibrationsToLoad;
01795 for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
01796 {
01797
01798
01799 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
01800 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01801
01802 int visualWordId = 0;
01803 int descriptorSize = 0;
01804 const void * descriptor = 0;
01805 int dRealSize = 0;
01806 cv::KeyPoint kpt;
01807 std::multimap<int, cv::KeyPoint> visualWords;
01808 std::multimap<int, cv::Point3f> visualWords3;
01809 std::multimap<int, cv::Mat> descriptors;
01810 cv::Point3f depth(0,0,0);
01811
01812
01813 rc = sqlite3_step(ppStmt);
01814 while(rc == SQLITE_ROW)
01815 {
01816 int index = 0;
01817 visualWordId = sqlite3_column_int(ppStmt, index++);
01818 kpt.pt.x = sqlite3_column_double(ppStmt, index++);
01819 kpt.pt.y = sqlite3_column_double(ppStmt, index++);
01820 kpt.size = sqlite3_column_int(ppStmt, index++);
01821 kpt.angle = sqlite3_column_double(ppStmt, index++);
01822 kpt.response = sqlite3_column_double(ppStmt, index++);
01823 depth.x = sqlite3_column_double(ppStmt, index++);
01824 depth.y = sqlite3_column_double(ppStmt, index++);
01825 depth.z = sqlite3_column_double(ppStmt, index++);
01826
01827 visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
01828 visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
01829
01830 if(uStrNumCmp(_version, "0.11.2") >= 0)
01831 {
01832 descriptorSize = sqlite3_column_int(ppStmt, index++);
01833 descriptor = sqlite3_column_blob(ppStmt, index);
01834 dRealSize = sqlite3_column_bytes(ppStmt, index++);
01835
01836 if(descriptor && descriptorSize>0 && dRealSize>0)
01837 {
01838 cv::Mat d;
01839 if(dRealSize == descriptorSize)
01840 {
01841
01842 d = cv::Mat(1, descriptorSize, CV_8U);
01843 }
01844 else if(dRealSize/int(sizeof(float)) == descriptorSize)
01845 {
01846
01847 d = cv::Mat(1, descriptorSize, CV_32F);
01848 }
01849 else
01850 {
01851 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
01852 }
01853
01854 memcpy(d.data, descriptor, dRealSize);
01855
01856 descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
01857 }
01858 }
01859
01860 rc = sqlite3_step(ppStmt);
01861 }
01862 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01863
01864 if(visualWords.size()==0)
01865 {
01866 UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
01867 }
01868 else
01869 {
01870 calibrationsToLoad.insert((*iter)->id());
01871 (*iter)->setWords(visualWords);
01872 (*iter)->setWords3(visualWords3);
01873 (*iter)->setWordsDescriptors(descriptors);
01874 ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), (int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
01875 }
01876
01877
01878 rc = sqlite3_reset(ppStmt);
01879 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01880 }
01881
01882
01883 rc = sqlite3_finalize(ppStmt);
01884 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01885
01886 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01887
01888 this->loadLinksQuery(nodes);
01889 for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
01890 {
01891 (*iter)->setModified(false);
01892 }
01893 ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
01894
01895
01896 if(calibrationsToLoad.size() && uStrNumCmp(_version, "0.10.0") >= 0)
01897 {
01898 std::stringstream query3;
01899 query3 << "SELECT calibration "
01900 "FROM Data "
01901 "WHERE id = ? ";
01902
01903 rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
01904 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01905
01906 int calibrationsLoaded = 0;
01907 for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
01908 {
01909 if(calibrationsToLoad.find((*iter)->id())!=calibrationsToLoad.end())
01910 {
01911
01912 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
01913 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01914
01915 rc = sqlite3_step(ppStmt);
01916 if(rc == SQLITE_ROW)
01917 {
01918 int index=0;
01919 const void * data = 0;
01920 int dataSize = 0;
01921 Transform localTransform;
01922 std::vector<CameraModel> models;
01923 StereoCameraModel stereoModel;
01924
01925
01926 data = sqlite3_column_blob(ppStmt, index);
01927 dataSize = sqlite3_column_bytes(ppStmt, index++);
01928
01929
01930 if(dataSize > 0 && data)
01931 {
01932 ++calibrationsLoaded;
01933 float * dataFloat = (float*)data;
01934 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
01935 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
01936 {
01937 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
01938 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01939 int max = cameraCount*(6+localTransform.size());
01940 for(int i=0; i<max; i+=6+localTransform.size())
01941 {
01942
01943 localTransform = Transform::getIdentity();
01944 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
01945 models.push_back(CameraModel(
01946 (double)dataFloat[i],
01947 (double)dataFloat[i+1],
01948 (double)dataFloat[i+2],
01949 (double)dataFloat[i+3],
01950 localTransform));
01951 models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
01952 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
01953 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
01954 localTransform.prettyPrint().c_str());
01955 }
01956 }
01957 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
01958 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
01959 {
01960 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
01961 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01962 int max = cameraCount*(4+localTransform.size());
01963 for(int i=0; i<max; i+=4+localTransform.size())
01964 {
01965
01966 localTransform = Transform::getIdentity();
01967 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
01968 models.push_back(CameraModel(
01969 (double)dataFloat[i],
01970 (double)dataFloat[i+1],
01971 (double)dataFloat[i+2],
01972 (double)dataFloat[i+3],
01973 localTransform));
01974 }
01975 }
01976 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
01977 {
01978 UDEBUG("Loading calibration of a stereo camera");
01979 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
01980 stereoModel = StereoCameraModel(
01981 dataFloat[0],
01982 dataFloat[1],
01983 dataFloat[2],
01984 dataFloat[3],
01985 dataFloat[4],
01986 localTransform);
01987 }
01988 else
01989 {
01990 UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
01991 }
01992
01993 (*iter)->sensorData().setCameraModels(models);
01994 (*iter)->sensorData().setStereoCameraModel(stereoModel);
01995 }
01996 rc = sqlite3_step(ppStmt);
01997 }
01998 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01999
02000
02001 rc = sqlite3_reset(ppStmt);
02002 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02003 }
02004 }
02005
02006 rc = sqlite3_finalize(ppStmt);
02007 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02008
02009 ULOGGER_DEBUG("Time load calibrations (loaded=%d/%d)=%fs", calibrationsLoaded, calibrationsToLoad.size(), timer.ticks());
02010 }
02011
02012 if(ids.size() != loaded)
02013 {
02014 UERROR("Some signatures not found in database");
02015 }
02016 }
02017 }
02018
02019 void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
02020 {
02021 ULOGGER_DEBUG("");
02022 if(_ppDb)
02023 {
02024 std::string type;
02025 UTimer timer;
02026 timer.start();
02027 int rc = SQLITE_OK;
02028 sqlite3_stmt * ppStmt = 0;
02029 std::string query;
02030 std::list<int> ids;
02031
02032 query = "SELECT n.id "
02033 "FROM Node AS n "
02034 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
02035 "ORDER BY n.id;";
02036
02037 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02038 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02039
02040
02041 rc = sqlite3_step(ppStmt);
02042 while(rc == SQLITE_ROW)
02043 {
02044 ids.push_back(sqlite3_column_int(ppStmt, 0));
02045 rc = sqlite3_step(ppStmt);
02046 }
02047
02048 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02049
02050
02051 rc = sqlite3_finalize(ppStmt);
02052 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02053
02054 ULOGGER_DEBUG("Loading %d signatures...", ids.size());
02055 this->loadSignaturesQuery(ids, nodes);
02056 ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
02057 }
02058 }
02059
02060 void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
02061 {
02062 ULOGGER_DEBUG("");
02063 if(_ppDb && dictionary)
02064 {
02065 std::string type;
02066 UTimer timer;
02067 timer.start();
02068 int rc = SQLITE_OK;
02069 sqlite3_stmt * ppStmt = 0;
02070 std::string query;
02071 std::list<VisualWord *> visualWords;
02072
02073
02074 query = "SELECT id, descriptor_size, descriptor "
02075 "FROM Word "
02076 "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
02077 "ORDER BY id;";
02078
02079 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02080 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02081
02082
02083 int id = 0;
02084 int descriptorSize = 0;
02085 const void * descriptor = 0;
02086 int dRealSize = 0;
02087 rc = sqlite3_step(ppStmt);
02088 int count = 0;
02089 while(rc == SQLITE_ROW)
02090 {
02091 int index=0;
02092 id = sqlite3_column_int(ppStmt, index++);
02093 if(id>0)
02094 {
02095 descriptorSize = sqlite3_column_int(ppStmt, index++);
02096 descriptor = sqlite3_column_blob(ppStmt, index);
02097 dRealSize = sqlite3_column_bytes(ppStmt, index++);
02098
02099 cv::Mat d;
02100 if(dRealSize == descriptorSize)
02101 {
02102
02103 d = cv::Mat(1, descriptorSize, CV_8U);
02104 }
02105 else if(dRealSize/int(sizeof(float)) == descriptorSize)
02106 {
02107
02108 d = cv::Mat(1, descriptorSize, CV_32F);
02109 }
02110 else
02111 {
02112 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
02113 }
02114
02115 memcpy(d.data, descriptor, dRealSize);
02116 VisualWord * vw = new VisualWord(id, d);
02117 vw->setSaved(true);
02118 dictionary->addWord(vw);
02119 }
02120 else
02121 {
02122 ULOGGER_ERROR("Wrong word id ?!? (%d)", id);
02123 }
02124 if(++count % 5000 == 0)
02125 {
02126 ULOGGER_DEBUG("Loaded %d words...", count);
02127 }
02128 rc = sqlite3_step(ppStmt);
02129 }
02130 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02131
02132 rc = sqlite3_finalize(ppStmt);
02133 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02134
02135
02136 getLastWordId(id);
02137 dictionary->setLastWordId(id);
02138
02139 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02140 }
02141 }
02142
02143
02144 void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
02145 {
02146 ULOGGER_DEBUG("size=%d", wordIds.size());
02147 if(_ppDb && wordIds.size())
02148 {
02149 std::string type;
02150 UTimer timer;
02151 timer.start();
02152 int rc = SQLITE_OK;
02153 sqlite3_stmt * ppStmt = 0;
02154 std::stringstream query;
02155 std::set<int> loaded;
02156
02157
02158 query << "SELECT vw.descriptor_size, vw.descriptor "
02159 "FROM Word as vw "
02160 "WHERE vw.id = ?;";
02161
02162 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02163 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02164
02165 int descriptorSize;
02166 const void * descriptor;
02167 int dRealSize;
02168 for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
02169 {
02170
02171 rc = sqlite3_bind_int(ppStmt, 1, *iter);
02172 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02173
02174
02175 rc = sqlite3_step(ppStmt);
02176 if(rc == SQLITE_ROW)
02177 {
02178 int index = 0;
02179 descriptorSize = sqlite3_column_int(ppStmt, index++);
02180 descriptor = sqlite3_column_blob(ppStmt, index);
02181 dRealSize = sqlite3_column_bytes(ppStmt, index++);
02182
02183 cv::Mat d;
02184 if(dRealSize == descriptorSize)
02185 {
02186
02187 d = cv::Mat(1, descriptorSize, CV_8U);
02188 }
02189 else if(dRealSize/int(sizeof(float)) == descriptorSize)
02190 {
02191
02192 d = cv::Mat(1, descriptorSize, CV_32F);
02193 }
02194 else
02195 {
02196 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
02197 }
02198
02199 memcpy(d.data, descriptor, dRealSize);
02200 VisualWord * vw = new VisualWord(*iter, d);
02201 if(vw)
02202 {
02203 vw->setSaved(true);
02204 }
02205 vws.push_back(vw);
02206 loaded.insert(loaded.end(), *iter);
02207
02208 rc = sqlite3_step(ppStmt);
02209 }
02210
02211 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02212
02213 rc = sqlite3_reset(ppStmt);
02214 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02215 }
02216
02217
02218 rc = sqlite3_finalize(ppStmt);
02219 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02220
02221 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02222
02223 if(wordIds.size() != loaded.size())
02224 {
02225 for(std::set<int>::const_iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
02226 {
02227 if(loaded.find(*iter) == loaded.end())
02228 {
02229 UDEBUG("Not found word %d", *iter);
02230 }
02231 }
02232 UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
02233 }
02234 }
02235 }
02236
02237 void DBDriverSqlite3::loadLinksQuery(
02238 int signatureId,
02239 std::map<int, Link> & neighbors,
02240 Link::Type typeIn) const
02241 {
02242 neighbors.clear();
02243 if(_ppDb)
02244 {
02245 UTimer timer;
02246 timer.start();
02247 int rc = SQLITE_OK;
02248 sqlite3_stmt * ppStmt = 0;
02249 std::stringstream query;
02250
02251 if(uStrNumCmp(_version, "0.10.10") >= 0)
02252 {
02253 query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
02254 }
02255 else if(uStrNumCmp(_version, "0.8.4") >= 0)
02256 {
02257 query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
02258 }
02259 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02260 {
02261 query << "SELECT to_id, type, transform, variance FROM Link ";
02262 }
02263 else
02264 {
02265 query << "SELECT to_id, type, transform FROM Link ";
02266 }
02267 query << "WHERE from_id = " << signatureId;
02268 if(typeIn != Link::kUndef)
02269 {
02270 if(uStrNumCmp(_version, "0.7.4") >= 0)
02271 {
02272 query << " AND type = " << typeIn;
02273 }
02274 else if(typeIn == Link::kNeighbor)
02275 {
02276 query << " AND type = 0";
02277 }
02278 else if(typeIn > Link::kNeighbor)
02279 {
02280 query << " AND type > 0";
02281 }
02282 }
02283 query << " ORDER BY to_id";
02284
02285 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02286 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02287
02288 int toId = -1;
02289 int type = Link::kUndef;
02290 float rotVariance = 1.0f;
02291 float transVariance = 1.0f;
02292 const void * data = 0;
02293 int dataSize = 0;
02294
02295
02296 rc = sqlite3_step(ppStmt);
02297 while(rc == SQLITE_ROW)
02298 {
02299 int index = 0;
02300
02301 toId = sqlite3_column_int(ppStmt, index++);
02302 type = sqlite3_column_int(ppStmt, index++);
02303
02304 data = sqlite3_column_blob(ppStmt, index);
02305 dataSize = sqlite3_column_bytes(ppStmt, index++);
02306
02307 Transform transform;
02308 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
02309 {
02310 memcpy(transform.data(), data, dataSize);
02311 }
02312 else if(dataSize)
02313 {
02314 UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
02315 }
02316
02317 if(uStrNumCmp(_version, "0.8.4") >= 0)
02318 {
02319 rotVariance = sqlite3_column_double(ppStmt, index++);
02320 transVariance = sqlite3_column_double(ppStmt, index++);
02321
02322 cv::Mat userDataCompressed;
02323 if(uStrNumCmp(_version, "0.10.10") >= 0)
02324 {
02325 const void * data = sqlite3_column_blob(ppStmt, index);
02326 dataSize = sqlite3_column_bytes(ppStmt, index++);
02327
02328 if(dataSize>4 && data)
02329 {
02330 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
02331 }
02332 }
02333
02334 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
02335 }
02336 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02337 {
02338 rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
02339 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
02340 }
02341 else
02342 {
02343
02344 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
02345 }
02346
02347 rc = sqlite3_step(ppStmt);
02348 }
02349
02350 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02351
02352
02353 rc = sqlite3_finalize(ppStmt);
02354 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02355
02356 if(neighbors.size() == 0)
02357 {
02358
02359 }
02360 }
02361 }
02362
02363 void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
02364 {
02365 if(_ppDb)
02366 {
02367 UTimer timer;
02368 timer.start();
02369 int rc = SQLITE_OK;
02370 sqlite3_stmt * ppStmt = 0;
02371 std::stringstream query;
02372 int totalLinksLoaded = 0;
02373
02374 if(uStrNumCmp(_version, "0.10.10") >= 0)
02375 {
02376 query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
02377 << "WHERE from_id = ? "
02378 << "ORDER BY to_id";
02379 }
02380 else if(uStrNumCmp(_version, "0.8.4") >= 0)
02381 {
02382 query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
02383 << "WHERE from_id = ? "
02384 << "ORDER BY to_id";
02385 }
02386 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02387 {
02388 query << "SELECT to_id, type, variance, transform FROM Link "
02389 << "WHERE from_id = ? "
02390 << "ORDER BY to_id";
02391 }
02392 else
02393 {
02394 query << "SELECT to_id, type, transform FROM Link "
02395 << "WHERE from_id = ? "
02396 << "ORDER BY to_id";
02397 }
02398
02399 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02400 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02401
02402 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
02403 {
02404
02405 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
02406 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02407
02408 int toId = -1;
02409 int linkType = -1;
02410 float rotVariance = 1.0f;
02411 float transVariance = 1.0f;
02412 std::list<Link> links;
02413 const void * data = 0;
02414 int dataSize = 0;
02415
02416
02417 rc = sqlite3_step(ppStmt);
02418 while(rc == SQLITE_ROW)
02419 {
02420 int index = 0;
02421
02422 toId = sqlite3_column_int(ppStmt, index++);
02423 linkType = sqlite3_column_int(ppStmt, index++);
02424 cv::Mat userDataCompressed;
02425 if(uStrNumCmp(_version, "0.8.4") >= 0)
02426 {
02427 rotVariance = sqlite3_column_double(ppStmt, index++);
02428 transVariance = sqlite3_column_double(ppStmt, index++);
02429
02430 if(uStrNumCmp(_version, "0.10.10") >= 0)
02431 {
02432 const void * data = sqlite3_column_blob(ppStmt, index);
02433 dataSize = sqlite3_column_bytes(ppStmt, index++);
02434
02435 if(dataSize>4 && data)
02436 {
02437 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
02438 }
02439 }
02440 }
02441 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02442 {
02443 rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
02444 }
02445
02446
02447 data = sqlite3_column_blob(ppStmt, index);
02448 dataSize = sqlite3_column_bytes(ppStmt, index++);
02449 Transform transform;
02450 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
02451 {
02452 memcpy(transform.data(), data, dataSize);
02453 }
02454 else if(dataSize)
02455 {
02456 UERROR("Error while loading link transform from %d to %d! Setting to null...", (*iter)->id(), toId);
02457 }
02458
02459 if(linkType >= 0 && linkType != Link::kUndef)
02460 {
02461 if(uStrNumCmp(_version, "0.7.4") >= 0)
02462 {
02463 links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
02464 }
02465 else
02466 {
02467 links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
02468 }
02469 }
02470 else
02471 {
02472 UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
02473 linkType, (*iter)->id(), toId);
02474 }
02475
02476 ++totalLinksLoaded;
02477 rc = sqlite3_step(ppStmt);
02478 }
02479 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02480
02481
02482 (*iter)->addLinks(links);
02483
02484
02485 rc = sqlite3_reset(ppStmt);
02486 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02487 UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
02488 }
02489
02490
02491 rc = sqlite3_finalize(ppStmt);
02492 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02493 }
02494 }
02495
02496
02497 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
02498 {
02499 UDEBUG("nodes = %d", nodes.size());
02500 if(_ppDb && nodes.size())
02501 {
02502 UTimer timer;
02503 timer.start();
02504 int rc = SQLITE_OK;
02505 sqlite3_stmt * ppStmt = 0;
02506 Signature * s = 0;
02507
02508 std::string query;
02509 if(uStrNumCmp(_version, "0.8.5") >= 0)
02510 {
02511 if(updateTimestamp)
02512 {
02513 query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
02514 }
02515 else
02516 {
02517 query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
02518 }
02519 }
02520 else
02521 {
02522 if(updateTimestamp)
02523 {
02524 query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
02525 }
02526 else
02527 {
02528 query = "UPDATE Node SET weight=? WHERE id=?;";
02529 }
02530 }
02531 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02532 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02533
02534 for(std::list<Signature *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
02535 {
02536 s = *i;
02537 int index = 1;
02538 if(s)
02539 {
02540 rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
02541 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02542
02543 if(uStrNumCmp(_version, "0.8.5") >= 0)
02544 {
02545 if(s->getLabel().empty())
02546 {
02547 rc = sqlite3_bind_null(ppStmt, index++);
02548 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02549 }
02550 else
02551 {
02552 rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
02553 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02554 }
02555 }
02556
02557 rc = sqlite3_bind_int(ppStmt, index++, s->id());
02558 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02559
02560
02561 rc=sqlite3_step(ppStmt);
02562 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02563
02564 rc = sqlite3_reset(ppStmt);
02565 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02566 }
02567 }
02568
02569 rc = sqlite3_finalize(ppStmt);
02570 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02571
02572 ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
02573
02574
02575 query = "DELETE FROM Link WHERE from_id=?;";
02576 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02577 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02578 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
02579 {
02580 if((*j)->isLinksModified())
02581 {
02582 rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
02583 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02584
02585 rc=sqlite3_step(ppStmt);
02586 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02587
02588 rc = sqlite3_reset(ppStmt);
02589 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02590 }
02591 }
02592
02593 rc = sqlite3_finalize(ppStmt);
02594 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02595
02596
02597 query = queryStepLink();
02598 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02599 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02600 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
02601 {
02602 if((*j)->isLinksModified())
02603 {
02604
02605 const std::map<int, Link> & links = (*j)->getLinks();
02606 for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
02607 {
02608 stepLink(ppStmt, i->second);
02609 }
02610 }
02611 }
02612
02613 rc = sqlite3_finalize(ppStmt);
02614 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02615 ULOGGER_DEBUG("Update Neighbors Time=%fs", timer.ticks());
02616
02617
02618 query = queryStepWordsChanged();
02619 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02620 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02621 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
02622 {
02623 if((*j)->getWordsChanged().size())
02624 {
02625 const std::map<int, int> & wordsChanged = (*j)->getWordsChanged();
02626 for(std::map<int, int>::const_iterator iter=wordsChanged.begin(); iter!=wordsChanged.end(); ++iter)
02627 {
02628 stepWordsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
02629 }
02630 }
02631 }
02632
02633 rc = sqlite3_finalize(ppStmt);
02634 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02635
02636 ULOGGER_DEBUG("signatures update=%fs", timer.ticks());
02637 }
02638 }
02639
02640 void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
02641 {
02642 if(_ppDb && words.size() && updateTimestamp)
02643 {
02644
02645 UTimer timer;
02646 timer.start();
02647 int rc = SQLITE_OK;
02648 sqlite3_stmt * ppStmt = 0;
02649 VisualWord * w = 0;
02650
02651 std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
02652 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02653 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02654
02655 for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
02656 {
02657 w = *i;
02658 int index = 1;
02659 if(w)
02660 {
02661 rc = sqlite3_bind_int(ppStmt, index++, w->id());
02662 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02663
02664
02665 rc=sqlite3_step(ppStmt);
02666 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02667
02668 rc = sqlite3_reset(ppStmt);
02669 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02670 }
02671 }
02672
02673 rc = sqlite3_finalize(ppStmt);
02674 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02675
02676 ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
02677 }
02678 }
02679
02680 void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
02681 {
02682 UDEBUG("");
02683 if(_ppDb && signatures.size())
02684 {
02685 std::string type;
02686 UTimer timer;
02687 timer.start();
02688 int rc = SQLITE_OK;
02689 sqlite3_stmt * ppStmt = 0;
02690
02691
02692 std::string query = queryStepNode();
02693 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02694 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02695
02696 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
02697 {
02698 stepNode(ppStmt, *i);
02699 }
02700
02701 rc = sqlite3_finalize(ppStmt);
02702 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02703
02704 UDEBUG("Time=%fs", timer.ticks());
02705
02706
02707 query = queryStepLink();
02708 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02709 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02710 for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
02711 {
02712
02713 const std::map<int, Link> & links = (*jter)->getLinks();
02714 for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
02715 {
02716 stepLink(ppStmt, i->second);
02717 }
02718 }
02719
02720 rc = sqlite3_finalize(ppStmt);
02721 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02722
02723 UDEBUG("Time=%fs", timer.ticks());
02724
02725
02726
02727 query = queryStepKeypoint();
02728 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02729 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02730 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
02731 {
02732 UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
02733 UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
02734
02735 std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
02736 std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
02737 for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
02738 {
02739 cv::Point3f pt(0,0,0);
02740 if(p!=(*i)->getWords3().end())
02741 {
02742 UASSERT(w->first == p->first);
02743 pt = p->second;
02744 ++p;
02745 }
02746
02747 cv::Mat descriptor;
02748 if(d!=(*i)->getWordsDescriptors().end())
02749 {
02750 UASSERT(w->first == d->first);
02751 descriptor = d->second;
02752 ++d;
02753 }
02754
02755 stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
02756 }
02757 }
02758
02759 rc = sqlite3_finalize(ppStmt);
02760 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02761 UDEBUG("Time=%fs", timer.ticks());
02762
02763 if(uStrNumCmp(_version, "0.10.0") >= 0)
02764 {
02765
02766 query = queryStepSensorData();
02767 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02768 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02769 UDEBUG("Saving %d images", signatures.size());
02770
02771 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
02772 {
02773 if(!(*i)->sensorData().imageCompressed().empty() ||
02774 !(*i)->sensorData().depthOrRightCompressed().empty() ||
02775 !(*i)->sensorData().laserScanCompressed().empty() ||
02776 !(*i)->sensorData().userDataCompressed().empty() ||
02777 !(*i)->sensorData().cameraModels().size() ||
02778 !(*i)->sensorData().stereoCameraModel().isValidForProjection())
02779 {
02780 UASSERT((*i)->id() == (*i)->sensorData().id());
02781 stepSensorData(ppStmt, (*i)->sensorData());
02782 }
02783 }
02784
02785
02786 rc = sqlite3_finalize(ppStmt);
02787 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02788 UDEBUG("Time=%fs", timer.ticks());
02789 }
02790 else
02791 {
02792
02793 query = queryStepImage();
02794 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02795 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02796 UDEBUG("Saving %d images", signatures.size());
02797
02798 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
02799 {
02800 if(!(*i)->sensorData().imageCompressed().empty())
02801 {
02802 stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
02803 }
02804 }
02805
02806
02807 rc = sqlite3_finalize(ppStmt);
02808 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02809 UDEBUG("Time=%fs", timer.ticks());
02810
02811
02812 query = queryStepDepth();
02813 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02814 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02815 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
02816 {
02817
02818 if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty())
02819 {
02820 UASSERT((*i)->id() == (*i)->sensorData().id());
02821 stepDepth(ppStmt, (*i)->sensorData());
02822 }
02823 }
02824
02825 rc = sqlite3_finalize(ppStmt);
02826 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02827 }
02828
02829 UDEBUG("Time=%fs", timer.ticks());
02830 }
02831 }
02832
02833 void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
02834 {
02835 UDEBUG("visualWords size=%d", words.size());
02836 if(_ppDb)
02837 {
02838 std::string type;
02839 UTimer timer;
02840 timer.start();
02841 int rc = SQLITE_OK;
02842 sqlite3_stmt * ppStmt = 0;
02843 std::string query;
02844
02845
02846 if(words.size()>0)
02847 {
02848 query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
02849 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02850 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02851 for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
02852 {
02853 const VisualWord * w = *iter;
02854 if(w && !w->isSaved())
02855 {
02856 rc = sqlite3_bind_int(ppStmt, 1, w->id());
02857 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02858 rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
02859 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02860 UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
02861 if(w->getDescriptor().type() == CV_32F)
02862 {
02863
02864 rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
02865 }
02866 else
02867 {
02868
02869 rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
02870 }
02871 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02872
02873
02874 rc=sqlite3_step(ppStmt);
02875 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02876
02877 rc = sqlite3_reset(ppStmt);
02878 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02879 }
02880 }
02881
02882 rc = sqlite3_finalize(ppStmt);
02883 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02884 }
02885
02886 UDEBUG("Time=%fs", timer.ticks());
02887 }
02888 }
02889
02890 void DBDriverSqlite3::addLinkQuery(const Link & link) const
02891 {
02892 UDEBUG("");
02893 if(_ppDb)
02894 {
02895 std::string type;
02896 UTimer timer;
02897 timer.start();
02898 int rc = SQLITE_OK;
02899 sqlite3_stmt * ppStmt = 0;
02900
02901
02902 std::string query = queryStepLink();
02903 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02904 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02905
02906
02907 stepLink(ppStmt, link);
02908
02909
02910 rc = sqlite3_finalize(ppStmt);
02911 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02912
02913 UDEBUG("Time=%fs", timer.ticks());
02914 }
02915
02916 }
02917
02918 void DBDriverSqlite3::updateLinkQuery(const Link & link) const
02919 {
02920 UDEBUG("");
02921 if(_ppDb)
02922 {
02923 std::string type;
02924 UTimer timer;
02925 timer.start();
02926 int rc = SQLITE_OK;
02927 sqlite3_stmt * ppStmt = 0;
02928
02929
02930 std::string query = queryStepLinkUpdate();
02931 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
02932 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02933
02934
02935 stepLink(ppStmt, link);
02936
02937
02938 rc = sqlite3_finalize(ppStmt);
02939 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02940
02941 UDEBUG("Time=%fs", timer.ticks());
02942 }
02943 }
02944
02945 std::string DBDriverSqlite3::queryStepNode() const
02946 {
02947 if(uStrNumCmp(_version, "0.11.1") >= 0)
02948 {
02949 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
02950 }
02951 else if(uStrNumCmp(_version, "0.10.1") >= 0)
02952 {
02953 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
02954 }
02955 else if(uStrNumCmp(_version, "0.8.8") >= 0)
02956 {
02957 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
02958 }
02959 else if(uStrNumCmp(_version, "0.8.5") >= 0)
02960 {
02961 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
02962 }
02963 return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
02964 }
02965 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
02966 {
02967 UDEBUG("Save node %d", s->id());
02968 if(!ppStmt || !s)
02969 {
02970 UFATAL("");
02971 }
02972 int rc = SQLITE_OK;
02973
02974 int index = 1;
02975 rc = sqlite3_bind_int(ppStmt, index++, s->id());
02976 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02977 rc = sqlite3_bind_int(ppStmt, index++, s->mapId());
02978 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02979 rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
02980 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02981 rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
02982 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02983
02984 if(uStrNumCmp(_version, "0.8.5") >= 0)
02985 {
02986 rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
02987 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02988
02989 if(s->getLabel().empty())
02990 {
02991 rc = sqlite3_bind_null(ppStmt, index++);
02992 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02993 }
02994 else
02995 {
02996 rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
02997 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02998 }
02999 }
03000
03001 if(uStrNumCmp(_version, "0.10.1") >= 0)
03002 {
03003
03004 }
03005 else if(uStrNumCmp(_version, "0.8.8") >= 0)
03006 {
03007 if(s->sensorData().userDataCompressed().empty())
03008 {
03009 rc = sqlite3_bind_null(ppStmt, index++);
03010 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03011 }
03012 else
03013 {
03014 rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
03015 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03016 }
03017 }
03018
03019 if(uStrNumCmp(_version, "0.11.1") >= 0)
03020 {
03021 rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
03022 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03023 }
03024
03025
03026 rc=sqlite3_step(ppStmt);
03027 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03028
03029 rc = sqlite3_reset(ppStmt);
03030 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03031 }
03032
03033 std::string DBDriverSqlite3::queryStepImage() const
03034 {
03035 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
03036 return "INSERT INTO Image(id, data) VALUES(?,?);";
03037 }
03038 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
03039 int id,
03040 const cv::Mat & imageBytes) const
03041 {
03042 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
03043 UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
03044 if(!ppStmt)
03045 {
03046 UFATAL("");
03047 }
03048
03049 int rc = SQLITE_OK;
03050 int index = 1;
03051
03052 rc = sqlite3_bind_int(ppStmt, index++, id);
03053 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03054
03055 if(!imageBytes.empty())
03056 {
03057 rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
03058 }
03059 else
03060 {
03061 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
03062 }
03063 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03064
03065
03066 rc=sqlite3_step(ppStmt);
03067 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03068
03069 rc = sqlite3_reset(ppStmt);
03070 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03071 }
03072
03073 std::string DBDriverSqlite3::queryStepDepth() const
03074 {
03075 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
03076 if(uStrNumCmp(_version, "0.8.11") >= 0)
03077 {
03078 return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
03079 }
03080 else if(uStrNumCmp(_version, "0.7.0") >= 0)
03081 {
03082 return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
03083 }
03084 else
03085 {
03086 return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
03087 }
03088 }
03089 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
03090 {
03091 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
03092 UDEBUG("Save depth %d (size=%d) depth2d = %d",
03093 sensorData.id(),
03094 (int)sensorData.depthOrRightCompressed().cols,
03095 (int)sensorData.laserScanCompressed().cols);
03096 if(!ppStmt)
03097 {
03098 UFATAL("");
03099 }
03100
03101 int rc = SQLITE_OK;
03102 int index = 1;
03103
03104 rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
03105 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03106
03107 if(!sensorData.depthOrRightCompressed().empty())
03108 {
03109 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
03110 }
03111 else
03112 {
03113 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
03114 }
03115 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03116
03117 float fx=0, fyOrBaseline=0, cx=0, cy=0;
03118 Transform localTransform = Transform::getIdentity();
03119 if(sensorData.cameraModels().size())
03120 {
03121 UASSERT_MSG(sensorData.cameraModels().size() == 1,
03122 uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
03123
03124 fx = sensorData.cameraModels()[0].fx();
03125 fyOrBaseline = sensorData.cameraModels()[0].fy();
03126 cx = sensorData.cameraModels()[0].cx();
03127 cy = sensorData.cameraModels()[0].cy();
03128 localTransform = sensorData.cameraModels()[0].localTransform();
03129 }
03130 else if(sensorData.stereoCameraModel().isValidForProjection())
03131 {
03132 fx = sensorData.stereoCameraModel().left().fx();
03133 fyOrBaseline = sensorData.stereoCameraModel().baseline();
03134 cx = sensorData.stereoCameraModel().left().cx();
03135 cy = sensorData.stereoCameraModel().left().cy();
03136 localTransform = sensorData.stereoCameraModel().left().localTransform();
03137 }
03138
03139 if(uStrNumCmp(_version, "0.7.0") >= 0)
03140 {
03141 rc = sqlite3_bind_double(ppStmt, index++, fx);
03142 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03143 rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
03144 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03145 rc = sqlite3_bind_double(ppStmt, index++, cx);
03146 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03147 rc = sqlite3_bind_double(ppStmt, index++, cy);
03148 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03149 }
03150 else
03151 {
03152 rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
03153 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03154 }
03155
03156 rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
03157 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03158
03159 if(!sensorData.laserScanCompressed().empty())
03160 {
03161 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
03162 }
03163 else
03164 {
03165 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
03166 }
03167 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03168
03169 if(uStrNumCmp(_version, "0.8.11") >= 0)
03170 {
03171 rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
03172 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03173 }
03174
03175
03176 rc=sqlite3_step(ppStmt);
03177 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03178
03179 rc = sqlite3_reset(ppStmt);
03180 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03181 }
03182
03183 std::string DBDriverSqlite3::queryStepSensorData() const
03184 {
03185 UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
03186 if(uStrNumCmp(_version, "0.10.7") >= 0)
03187 {
03188 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
03189 }
03190 else if(uStrNumCmp(_version, "0.10.1") >= 0)
03191 {
03192 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
03193 }
03194 else
03195 {
03196 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
03197 }
03198 }
03199 void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
03200 const SensorData & sensorData) const
03201 {
03202 UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
03203 UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
03204 sensorData.id(),
03205 (int)sensorData.imageCompressed().cols,
03206 (int)sensorData.depthOrRightCompressed().cols,
03207 (int)sensorData.laserScanCompressed().cols);
03208 if(!ppStmt)
03209 {
03210 UFATAL("");
03211 }
03212
03213 int rc = SQLITE_OK;
03214 int index = 1;
03215
03216
03217 rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
03218 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03219
03220
03221 if(!sensorData.imageCompressed().empty())
03222 {
03223 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
03224 }
03225 else
03226 {
03227 rc = sqlite3_bind_null(ppStmt, index++);
03228 }
03229 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03230
03231
03232 if(!sensorData.depthOrRightCompressed().empty())
03233 {
03234 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
03235 }
03236 else
03237 {
03238 rc = sqlite3_bind_null(ppStmt, index++);
03239 }
03240 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03241
03242
03243 std::vector<float> calibration;
03244
03245
03246 if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
03247 {
03248 if(uStrNumCmp(_version, "0.11.2") >= 0)
03249 {
03250 calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
03251 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
03252 {
03253 UASSERT(sensorData.cameraModels()[i].isValidForProjection());
03254 const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
03255 calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
03256 calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
03257 calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
03258 calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
03259 calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
03260 calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
03261 memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
03262 }
03263 }
03264 else
03265 {
03266 calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
03267 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
03268 {
03269 UASSERT(sensorData.cameraModels()[i].isValidForProjection());
03270 const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
03271 calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
03272 calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
03273 calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
03274 calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
03275 memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
03276 }
03277 }
03278 }
03279 else if(sensorData.stereoCameraModel().isValidForProjection())
03280 {
03281 const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
03282 calibration.resize(5+localTransform.size());
03283 calibration[0] = sensorData.stereoCameraModel().left().fx();
03284 calibration[1] = sensorData.stereoCameraModel().left().fy();
03285 calibration[2] = sensorData.stereoCameraModel().left().cx();
03286 calibration[3] = sensorData.stereoCameraModel().left().cy();
03287 calibration[4] = sensorData.stereoCameraModel().baseline();
03288 memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
03289 }
03290
03291 if(calibration.size())
03292 {
03293 rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
03294 }
03295 else
03296 {
03297 rc = sqlite3_bind_null(ppStmt, index++);
03298 }
03299 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03300
03301
03302 rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
03303 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03304
03305
03306 if(uStrNumCmp(_version, "0.10.7") >= 0)
03307 {
03308 rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange());
03309 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03310 }
03311
03312
03313 if(!sensorData.laserScanCompressed().empty())
03314 {
03315 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
03316 }
03317 else
03318 {
03319 rc = sqlite3_bind_null(ppStmt, index++);
03320 }
03321 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03322
03323 if(uStrNumCmp(_version, "0.10.1") >= 0)
03324 {
03325
03326 if(!sensorData.userDataCompressed().empty())
03327 {
03328 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
03329 }
03330 else
03331 {
03332 rc = sqlite3_bind_null(ppStmt, index++);
03333 }
03334 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03335 }
03336
03337
03338 rc=sqlite3_step(ppStmt);
03339 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03340
03341 rc = sqlite3_reset(ppStmt);
03342 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03343 }
03344
03345 std::string DBDriverSqlite3::queryStepLinkUpdate() const
03346 {
03347 if(uStrNumCmp(_version, "0.10.10") >= 0)
03348 {
03349 return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
03350 }
03351 else if(uStrNumCmp(_version, "0.8.4") >= 0)
03352 {
03353 return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
03354 }
03355 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03356 {
03357 return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
03358 }
03359 else
03360 {
03361 return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
03362 }
03363 }
03364 std::string DBDriverSqlite3::queryStepLink() const
03365 {
03366
03367 if(uStrNumCmp(_version, "0.10.10") >= 0)
03368 {
03369 return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
03370 }
03371 else if(uStrNumCmp(_version, "0.8.4") >= 0)
03372 {
03373 return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
03374 }
03375 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03376 {
03377 return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
03378 }
03379 else
03380 {
03381 return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
03382 }
03383 }
03384 void DBDriverSqlite3::stepLink(
03385 sqlite3_stmt * ppStmt,
03386 const Link & link) const
03387 {
03388 if(!ppStmt)
03389 {
03390 UFATAL("");
03391 }
03392 UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
03393
03394
03395 if(link.type()==Link::kVirtualClosure)
03396 {
03397 UDEBUG("Virtual link ignored....");
03398 return;
03399 }
03400
03401 int rc = SQLITE_OK;
03402 int index = 1;
03403 rc = sqlite3_bind_int(ppStmt, index++, link.type());
03404 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03405
03406 if(uStrNumCmp(_version, "0.8.4") >= 0)
03407 {
03408 rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
03409 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03410 rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
03411 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03412 }
03413 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03414 {
03415 rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
03416 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03417 }
03418
03419 rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
03420 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03421
03422 if(uStrNumCmp(_version, "0.10.10") >= 0)
03423 {
03424
03425 if(!link.userDataCompressed().empty())
03426 {
03427 rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
03428 }
03429 else
03430 {
03431 rc = sqlite3_bind_null(ppStmt, index++);
03432 }
03433 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03434 }
03435
03436 rc = sqlite3_bind_int(ppStmt, index++, link.from());
03437 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03438 rc = sqlite3_bind_int(ppStmt, index++, link.to());
03439 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03440
03441 rc=sqlite3_step(ppStmt);
03442 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03443
03444 rc=sqlite3_reset(ppStmt);
03445 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03446 }
03447
03448 std::string DBDriverSqlite3::queryStepWordsChanged() const
03449 {
03450 return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
03451 }
03452 void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
03453 {
03454 if(!ppStmt)
03455 {
03456 UFATAL("");
03457 }
03458 int rc = SQLITE_OK;
03459 int index = 1;
03460 rc = sqlite3_bind_int(ppStmt, index++, newWordId);
03461 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03462 rc = sqlite3_bind_int(ppStmt, index++, oldWordId);
03463 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03464 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
03465 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03466
03467 rc=sqlite3_step(ppStmt);
03468 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03469
03470 rc=sqlite3_reset(ppStmt);
03471 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03472 }
03473
03474 std::string DBDriverSqlite3::queryStepKeypoint() const
03475 {
03476 if(uStrNumCmp(_version, "0.11.2") >= 0)
03477 {
03478 return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?);";
03479 }
03480 return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
03481 }
03482 void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
03483 int nodeId,
03484 int wordId,
03485 const cv::KeyPoint & kp,
03486 const cv::Point3f & pt,
03487 const cv::Mat & descriptor) const
03488 {
03489 if(!ppStmt)
03490 {
03491 UFATAL("");
03492 }
03493 int rc = SQLITE_OK;
03494 int index = 1;
03495 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
03496 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03497 rc = sqlite3_bind_int(ppStmt, index++, wordId);
03498 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03499 rc = sqlite3_bind_double(ppStmt, index++, kp.pt.x);
03500 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03501 rc = sqlite3_bind_double(ppStmt, index++, kp.pt.y);
03502 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03503 rc = sqlite3_bind_int(ppStmt, index++, kp.size);
03504 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03505 rc = sqlite3_bind_double(ppStmt, index++, kp.angle);
03506 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03507 rc = sqlite3_bind_double(ppStmt, index++, kp.response);
03508 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03509 rc = sqlite3_bind_double(ppStmt, index++, pt.x);
03510 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03511 rc = sqlite3_bind_double(ppStmt, index++, pt.y);
03512 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03513 rc = sqlite3_bind_double(ppStmt, index++, pt.z);
03514 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03515
03516
03517 if(uStrNumCmp(_version, "0.11.2") >= 0)
03518 {
03519 rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
03520 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03521 UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
03522 if(descriptor.empty())
03523 {
03524 rc = sqlite3_bind_null(ppStmt, index++);
03525 }
03526 else
03527 {
03528 if(descriptor.type() == CV_32F)
03529 {
03530
03531 rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
03532 }
03533 else
03534 {
03535
03536 rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
03537 }
03538 }
03539 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03540 }
03541
03542 rc=sqlite3_step(ppStmt);
03543 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03544
03545 rc = sqlite3_reset(ppStmt);
03546 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03547 }
03548
03549 }