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 "rtabmap/core/DBDriverSqlite3.h"
00029 #include <sqlite3.h>
00030
00031 #include "rtabmap/core/Signature.h"
00032 #include "rtabmap/core/VisualWord.h"
00033 #include "rtabmap/core/VWDictionary.h"
00034 #include "rtabmap/core/util3d.h"
00035 #include "rtabmap/core/Compression.h"
00036 #include "DatabaseSchema_sql.h"
00037 #include <set>
00038
00039 #include "rtabmap/utilite/UtiLite.h"
00040
00041 namespace rtabmap {
00042
00043 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
00044 DBDriver(parameters),
00045 _ppDb(0),
00046 _version("0.0.0"),
00047 _memoryUsedEstimate(0),
00048 _dbInMemory(Parameters::defaultDbSqlite3InMemory()),
00049 _cacheSize(Parameters::defaultDbSqlite3CacheSize()),
00050 _journalMode(Parameters::defaultDbSqlite3JournalMode()),
00051 _synchronous(Parameters::defaultDbSqlite3Synchronous()),
00052 _tempStore(Parameters::defaultDbSqlite3TempStore())
00053 {
00054 ULOGGER_DEBUG("treadSafe=%d", sqlite3_threadsafe());
00055 this->parseParameters(parameters);
00056 }
00057
00058 DBDriverSqlite3::~DBDriverSqlite3()
00059 {
00060 this->closeConnection();
00061 }
00062
00063 void DBDriverSqlite3::parseParameters(const ParametersMap & parameters)
00064 {
00065 ParametersMap::const_iterator iter;
00066 if((iter=parameters.find(Parameters::kDbSqlite3CacheSize())) != parameters.end())
00067 {
00068 this->setCacheSize(std::atoi((*iter).second.c_str()));
00069 }
00070 if((iter=parameters.find(Parameters::kDbSqlite3JournalMode())) != parameters.end())
00071 {
00072 this->setJournalMode(std::atoi((*iter).second.c_str()));
00073 }
00074 if((iter=parameters.find(Parameters::kDbSqlite3Synchronous())) != parameters.end())
00075 {
00076 this->setSynchronous(std::atoi((*iter).second.c_str()));
00077 }
00078 if((iter=parameters.find(Parameters::kDbSqlite3TempStore())) != parameters.end())
00079 {
00080 this->setTempStore(std::atoi((*iter).second.c_str()));
00081 }
00082 if((iter=parameters.find(Parameters::kDbSqlite3InMemory())) != parameters.end())
00083 {
00084 this->setDbInMemory(uStr2Bool((*iter).second.c_str()));
00085 }
00086 DBDriver::parseParameters(parameters);
00087 }
00088
00089 void DBDriverSqlite3::setCacheSize(unsigned int cacheSize)
00090 {
00091 if(this->isConnected())
00092 {
00093 _cacheSize = cacheSize;
00094 std::string query = "PRAGMA cache_size = ";
00095 query += uNumber2Str(_cacheSize) + ";";
00096 this->executeNoResultQuery(query.c_str());
00097 }
00098 }
00099
00100 void DBDriverSqlite3::setJournalMode(int journalMode)
00101 {
00102 if(journalMode >= 0 && journalMode < 5)
00103 {
00104 _journalMode = journalMode;
00105 if(this->isConnected())
00106 {
00107 switch(_journalMode)
00108 {
00109 case 4:
00110 this->executeNoResultQuery("PRAGMA journal_mode = OFF;");
00111 break;
00112 case 3:
00113 this->executeNoResultQuery("PRAGMA journal_mode = MEMORY;");
00114 break;
00115 case 2:
00116 this->executeNoResultQuery("PRAGMA journal_mode = PERSIST;");
00117 break;
00118 case 1:
00119 this->executeNoResultQuery("PRAGMA journal_mode = TRUNCATE;");
00120 break;
00121 case 0:
00122 default:
00123 this->executeNoResultQuery("PRAGMA journal_mode = DELETE;");
00124 break;
00125 }
00126 }
00127 }
00128 else
00129 {
00130 ULOGGER_ERROR("Wrong journal mode (%d)", journalMode);
00131 }
00132 }
00133
00134 void DBDriverSqlite3::setSynchronous(int synchronous)
00135 {
00136 if(synchronous >= 0 && synchronous < 3)
00137 {
00138 _synchronous = synchronous;
00139 if(this->isConnected())
00140 {
00141 switch(_synchronous)
00142 {
00143 case 0:
00144 this->executeNoResultQuery("PRAGMA synchronous = OFF;");
00145 break;
00146 case 1:
00147 this->executeNoResultQuery("PRAGMA synchronous = NORMAL;");
00148 break;
00149 case 2:
00150 default:
00151 this->executeNoResultQuery("PRAGMA synchronous = FULL;");
00152 break;
00153 }
00154 }
00155 }
00156 else
00157 {
00158 ULOGGER_ERROR("Wrong synchronous value (%d)", synchronous);
00159 }
00160 }
00161
00162 void DBDriverSqlite3::setTempStore(int tempStore)
00163 {
00164 if(tempStore >= 0 && tempStore < 3)
00165 {
00166 _tempStore = tempStore;
00167 if(this->isConnected())
00168 {
00169 switch(_tempStore)
00170 {
00171 case 2:
00172 this->executeNoResultQuery("PRAGMA temp_store = MEMORY;");
00173 break;
00174 case 1:
00175 this->executeNoResultQuery("PRAGMA temp_store = FILE;");
00176 break;
00177 case 0:
00178 default:
00179 this->executeNoResultQuery("PRAGMA temp_store = DEFAULT;");
00180 break;
00181 }
00182 }
00183 }
00184 else
00185 {
00186 ULOGGER_ERROR("Wrong tempStore value (%d)", tempStore);
00187 }
00188 }
00189
00190 void DBDriverSqlite3::setDbInMemory(bool dbInMemory)
00191 {
00192 UDEBUG("dbInMemory=%d", dbInMemory?1:0);
00193 if(dbInMemory != _dbInMemory)
00194 {
00195 if(this->isConnected())
00196 {
00197
00198 join(true);
00199 this->emptyTrashes();
00200 this->closeConnection();
00201 _dbInMemory = dbInMemory;
00202 this->openConnection(this->getUrl());
00203 }
00204 else
00205 {
00206 _dbInMemory = dbInMemory;
00207 }
00208 }
00209 }
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228 int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const
00229 {
00230 int rc;
00231 sqlite3 *pFile = 0;
00232 sqlite3_backup *pBackup = 0;
00233 sqlite3 *pTo = 0;
00234 sqlite3 *pFrom = 0;
00235
00236
00237
00238 rc = sqlite3_open(fileName.c_str(), &pFile);
00239 if( rc==SQLITE_OK ){
00240
00241
00242
00243
00244
00245
00246 pFrom = (isSave ? pInMemory : pFile);
00247 pTo = (isSave ? pFile : pInMemory);
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261 pBackup = sqlite3_backup_init(pTo, "main", pFrom, "main");
00262 if( pBackup ){
00263 (void)sqlite3_backup_step(pBackup, -1);
00264 (void)sqlite3_backup_finish(pBackup);
00265 }
00266 rc = sqlite3_errcode(pTo);
00267 }
00268
00269
00270
00271 (void)sqlite3_close(pFile);
00272 return rc;
00273 }
00274
00275 bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
00276 {
00277 version = "0.0.0";
00278 if(_ppDb)
00279 {
00280 UTimer timer;
00281 timer.start();
00282 int rc = SQLITE_OK;
00283 sqlite3_stmt * ppStmt = 0;
00284 std::stringstream query;
00285
00286 query << "SELECT version FROM Admin;";
00287
00288 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
00289 if(rc == SQLITE_OK)
00290 {
00291
00292 rc = sqlite3_step(ppStmt);
00293 if(rc == SQLITE_ROW)
00294 {
00295 version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
00296 rc = sqlite3_step(ppStmt);
00297 }
00298 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00299
00300
00301 rc = sqlite3_finalize(ppStmt);
00302 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00303 }
00304
00305
00306
00307
00308 return true;
00309 }
00310 return false;
00311 }
00312
00313 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
00314 {
00315 this->disconnectDatabaseQuery();
00316
00317 _ppDb = 0;
00318 _memoryUsedEstimate = 0;
00319
00320 int rc = SQLITE_OK;
00321 bool dbFileExist = false;
00322 if(!url.empty())
00323 {
00324 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 else if(dbFileExist)
00332 {
00333 _memoryUsedEstimate = UFile::length(this->getUrl());
00334 }
00335 }
00336
00337 if(_dbInMemory || url.empty())
00338 {
00339 if(!url.empty())
00340 {
00341 ULOGGER_INFO("Using database \"%s\" in the memory.", url.c_str());
00342 }
00343 else
00344 {
00345 ULOGGER_INFO("Using empty database in the memory.");
00346 }
00347 rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
00348 }
00349 else
00350 {
00351 ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
00352 rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
00353 }
00354 if(rc != SQLITE_OK)
00355 {
00356 UFATAL("DB error : %s (path=\"%s\"). Make sure that your user has write "
00357 "permission on the target directory (you may have to change the working directory). ", sqlite3_errmsg(_ppDb), url.c_str());
00358 _ppDb = 0;
00359 return false;
00360 }
00361
00362 if(_dbInMemory && dbFileExist)
00363 {
00364 UTimer timer;
00365 timer.start();
00366 ULOGGER_DEBUG("Loading DB ...");
00367 rc = loadOrSaveDb(_ppDb, url, 0);
00368 ULOGGER_INFO("Loading DB time = %fs, (%s)", timer.ticks(), url.c_str());
00369 if(rc != SQLITE_OK)
00370 {
00371 UFATAL("DB error 2 : %s", sqlite3_errmsg(_ppDb));
00372 sqlite3_close(_ppDb);
00373 _ppDb = 0;
00374 return false;
00375 }
00376 }
00377
00378 if(!dbFileExist)
00379 {
00380 if(!url.empty())
00381 {
00382 ULOGGER_INFO("Database \"%s\" doesn't exist, creating a new one...", url.c_str());
00383 }
00384
00385 std::string schema = DATABASESCHEMA_SQL;
00386 schema = uHex2Str(schema);
00387 this->executeNoResultQuery(schema.c_str());
00388 }
00389 UASSERT(this->getDatabaseVersionQuery(_version));
00390 UINFO("Database version = %s", _version.c_str());
00391
00392
00393 if((uStrNumCmp(_version, "0.11.12") <= 0 && uStrNumCmp(_version, RTABMAP_VERSION) > 0) ||
00394 (uStrNumCmp(_version, "0.11.12") > 0 && uStrNumCmp(RTABMAP_VERSION, "0.11.12") > 0 && uStrNumCmp(_version, uFormat("%d.%d.99", RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR)) > 0))
00395 {
00396 UERROR("Opened database version (%s) is more recent than rtabmap "
00397 "installed version (%s). Please update rtabmap to new version!",
00398 _version.c_str(), RTABMAP_VERSION);
00399 this->disconnectDatabaseQuery(false);
00400 return false;
00401 }
00402
00403
00404 this->setCacheSize(_cacheSize);
00405 this->setJournalMode(_journalMode);
00406 this->setSynchronous(_synchronous);
00407 this->setTempStore(_tempStore);
00408
00409 return true;
00410 }
00411 void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
00412 {
00413 UDEBUG("");
00414 if(_ppDb)
00415 {
00416 int rc = SQLITE_OK;
00417
00418 sqlite3_stmt * pStmt;
00419 while( (pStmt = sqlite3_next_stmt(_ppDb, 0))!=0 )
00420 {
00421 rc = sqlite3_finalize(pStmt);
00422 if(rc != SQLITE_OK)
00423 {
00424 UERROR("");
00425 }
00426 }
00427
00428 if(save && (_dbInMemory || this->getUrl().empty()))
00429 {
00430 UTimer timer;
00431 timer.start();
00432 std::string outputFile = this->getUrl();
00433 if(!outputUrl.empty())
00434 {
00435 outputFile = outputUrl;
00436 }
00437 if(outputFile.empty())
00438 {
00439 UERROR("Database was initialized with an empty url (in memory). To save it, "
00440 "the output url should not be empty. The database is thus closed without being saved!");
00441 }
00442 else
00443 {
00444 UINFO("Saving database to %s ...", outputFile.c_str());
00445 rc = loadOrSaveDb(_ppDb, outputFile, 1);
00446 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s), could not save \"%s\": %s. Make sure that your user has write "
00447 "permission on the target directory (you may have to change the working directory). ", _version.c_str(), outputFile.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00448 ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
00449 }
00450 }
00451
00452
00453 UINFO("Disconnecting database %s...", this->getUrl().c_str());
00454 sqlite3_close(_ppDb);
00455 _ppDb = 0;
00456
00457 if(save && !_dbInMemory && !outputUrl.empty() && !this->getUrl().empty() && outputUrl.compare(this->getUrl()) != 0)
00458 {
00459 UWARN("Output database path (%s) is different than the opened database "
00460 "path (%s). Opened database path is overwritten then renamed to output path.",
00461 outputUrl.c_str(), this->getUrl().c_str());
00462 if(UFile::rename(this->getUrl(), outputUrl) != 0)
00463 {
00464 UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
00465 }
00466 }
00467 }
00468 }
00469
00470 bool DBDriverSqlite3::isConnectedQuery() const
00471 {
00472 return _ppDb != 0;
00473 }
00474
00475
00476 void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
00477 {
00478 if(_ppDb)
00479 {
00480 UTimer timer;
00481 timer.start();
00482 int rc;
00483 rc = sqlite3_exec(_ppDb, sql.c_str(), 0, 0, 0);
00484 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s, the query is %s", sqlite3_errmsg(_ppDb), sql.c_str()).c_str());
00485 UDEBUG("Time=%fs", timer.ticks());
00486 }
00487 }
00488
00489 long DBDriverSqlite3::getMemoryUsedQuery() const
00490 {
00491 if(_dbInMemory)
00492 {
00493 return sqlite3_memory_used();
00494 }
00495 else
00496 {
00497 return _memoryUsedEstimate;
00498 }
00499 }
00500
00501 long DBDriverSqlite3::getNodesMemoryUsedQuery() const
00502 {
00503 UDEBUG("");
00504 long size = 0L;
00505 if(_ppDb)
00506 {
00507 std::string query;
00508 if(uStrNumCmp(_version, "0.14.0") >= 0)
00509 {
00510 query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose) + ifnull(length(velocity),0) + ifnull(length(gps),0) + length(time_enter)) from Node;";
00511 }
00512 else if(uStrNumCmp(_version, "0.13.0") >= 0)
00513 {
00514 query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose) + ifnull(length(velocity),0) + length(time_enter)) from Node;";
00515 }
00516 else if(uStrNumCmp(_version, "0.11.1") >= 0)
00517 {
00518 query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose)+ length(time_enter)) from Node;";
00519 }
00520 else if(uStrNumCmp(_version, "0.8.5") >= 0)
00521 {
00522 query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(time_enter)) from Node;";
00523 }
00524 else
00525 {
00526 query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose)+ length(time_enter)) from Node;";
00527 }
00528
00529 int rc = SQLITE_OK;
00530 sqlite3_stmt * ppStmt = 0;
00531 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00532 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00533 rc = sqlite3_step(ppStmt);
00534 if(rc == SQLITE_ROW)
00535 {
00536 size = sqlite3_column_int64(ppStmt, 0);
00537 rc = sqlite3_step(ppStmt);
00538 }
00539 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00540 rc = sqlite3_finalize(ppStmt);
00541 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00542 }
00543 return size;
00544 }
00545 long DBDriverSqlite3::getLinksMemoryUsedQuery() const
00546 {
00547 UDEBUG("");
00548 long size = 0L;
00549 if(_ppDb)
00550 {
00551 std::string query;
00552 if(uStrNumCmp(_version, "0.13.0") >= 0)
00553 {
00554 query = "SELECT sum(length(type) + length(information_matrix) + length(transform) + ifnull(length(user_data),0) + length(from_id) + length(to_id)) from Link;";
00555 }
00556 else if(uStrNumCmp(_version, "0.10.10") >= 0)
00557 {
00558 query = "SELECT sum(length(type) + length(rot_variance) + length(trans_variance) + length(transform) + ifnull(length(user_data),0) + length(from_id) + length(to_id)) from Link;";
00559 }
00560 else if(uStrNumCmp(_version, "0.8.4") >= 0)
00561 {
00562 query = "SELECT sum(length(type) + length(rot_variance) + length(trans_variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
00563 }
00564 else if(uStrNumCmp(_version, "0.7.4") >= 0)
00565 {
00566 query = "SELECT sum(length(type) + length(variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
00567 }
00568 else
00569 {
00570 query = "SELECT sum(length(type) + length(transform) + length(from_id) + length(to_id)) from Link;";
00571 }
00572
00573 int rc = SQLITE_OK;
00574 sqlite3_stmt * ppStmt = 0;
00575 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00576 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00577 rc = sqlite3_step(ppStmt);
00578 if(rc == SQLITE_ROW)
00579 {
00580 size = sqlite3_column_int64(ppStmt, 0);
00581 rc = sqlite3_step(ppStmt);
00582 }
00583 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00584 rc = sqlite3_finalize(ppStmt);
00585 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00586 }
00587 return size;
00588 }
00589 long DBDriverSqlite3::getImagesMemoryUsedQuery() const
00590 {
00591 UDEBUG("");
00592 long size = 0L;
00593 if(_ppDb)
00594 {
00595 std::string query;
00596 if(uStrNumCmp(_version, "0.10.0") >= 0)
00597 {
00598 query = "SELECT sum(ifnull(length(image),0) + ifnull(length(time_enter),0)) from Data;";
00599 }
00600 else
00601 {
00602 query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Image;";
00603 }
00604
00605 int rc = SQLITE_OK;
00606 sqlite3_stmt * ppStmt = 0;
00607 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00608 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00609 rc = sqlite3_step(ppStmt);
00610 if(rc == SQLITE_ROW)
00611 {
00612 size = sqlite3_column_int64(ppStmt, 0);
00613 rc = sqlite3_step(ppStmt);
00614 }
00615 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00616 rc = sqlite3_finalize(ppStmt);
00617 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00618 }
00619 return size;
00620 }
00621 long DBDriverSqlite3::getDepthImagesMemoryUsedQuery() const
00622 {
00623 UDEBUG("");
00624 long size = 0L;
00625 if(_ppDb)
00626 {
00627 std::string query;
00628 if(uStrNumCmp(_version, "0.10.0") >= 0)
00629 {
00630 query = "SELECT sum(ifnull(length(depth),0) + ifnull(length(time_enter),0)) from Data;";
00631 }
00632 else
00633 {
00634 query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Depth;";
00635 }
00636
00637 int rc = SQLITE_OK;
00638 sqlite3_stmt * ppStmt = 0;
00639 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00640 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00641 rc = sqlite3_step(ppStmt);
00642 if(rc == SQLITE_ROW)
00643 {
00644 size = sqlite3_column_int64(ppStmt, 0);
00645 rc = sqlite3_step(ppStmt);
00646 }
00647 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00648 rc = sqlite3_finalize(ppStmt);
00649 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00650 }
00651 return size;
00652 }
00653 long DBDriverSqlite3::getCalibrationsMemoryUsedQuery() const
00654 {
00655 UDEBUG("");
00656 long size = 0L;
00657 if(_ppDb)
00658 {
00659 std::string query;
00660
00661 if(uStrNumCmp(_version, "0.10.0") >= 0)
00662 {
00663 query = "SELECT sum(length(calibration)) from Data;";
00664 }
00665 else if(uStrNumCmp(_version, "0.7.0") >= 0)
00666 {
00667 query = "SELECT sum(length(fx) + length(fy) + length(cx) + length(cy) + length(local_transform)) from Depth;";
00668 }
00669 else
00670 {
00671 query = "SELECT sum(length(constant) + length(local_transform)) from Depth;";
00672 }
00673
00674 int rc = SQLITE_OK;
00675 sqlite3_stmt * ppStmt = 0;
00676 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00677 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00678 rc = sqlite3_step(ppStmt);
00679 if(rc == SQLITE_ROW)
00680 {
00681 size = sqlite3_column_int64(ppStmt, 0);
00682 rc = sqlite3_step(ppStmt);
00683 }
00684 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00685 rc = sqlite3_finalize(ppStmt);
00686 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00687 }
00688 return size;
00689 }
00690 long DBDriverSqlite3::getGridsMemoryUsedQuery() const
00691 {
00692 UDEBUG("");
00693 long size = 0L;
00694 if(_ppDb)
00695 {
00696 std::string query;
00697
00698 if(uStrNumCmp(_version, "0.11.10") >= 0)
00699 {
00700 query = "SELECT sum(ifnull(length(ground_cells),0) + ifnull(length(obstacle_cells),0) + length(cell_size) + length(view_point_x) + length(view_point_y) + length(view_point_z)) from Data;";
00701 }
00702 else
00703 {
00704 return size;
00705 }
00706
00707 int rc = SQLITE_OK;
00708 sqlite3_stmt * ppStmt = 0;
00709 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00710 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00711 rc = sqlite3_step(ppStmt);
00712 if(rc == SQLITE_ROW)
00713 {
00714 size = sqlite3_column_int64(ppStmt, 0);
00715 rc = sqlite3_step(ppStmt);
00716 }
00717 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00718 rc = sqlite3_finalize(ppStmt);
00719 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00720 }
00721 return size;
00722 }
00723 long DBDriverSqlite3::getLaserScansMemoryUsedQuery() const
00724 {
00725 UDEBUG("");
00726 long size = 0L;
00727 if(_ppDb)
00728 {
00729 std::string query;
00730
00731 if(uStrNumCmp(_version, "0.11.10") >= 0)
00732 {
00733 query = "SELECT sum(ifnull(length(scan_info),0) + ifnull(length(scan),0)) from Data;";
00734 }
00735 else if(uStrNumCmp(_version, "0.10.7") >= 0)
00736 {
00737 query = "SELECT sum(length(scan_max_pts) + length(scan_max_range) + ifnull(length(scan),0)) from Data;";
00738 }
00739 else if(uStrNumCmp(_version, "0.10.0") >= 0)
00740 {
00741 query = "SELECT sum(length(scan_max_pts) + ifnull(length(scan),0)) from Data;";
00742 }
00743 else if(uStrNumCmp(_version, "0.8.11") >= 0)
00744 {
00745 query = "SELECT sum(length(data2d) + length(data2d_max_pts)) from Depth;";
00746 }
00747 else
00748 {
00749 query = "SELECT sum(length(data2d)) from Depth;";
00750 }
00751
00752 int rc = SQLITE_OK;
00753 sqlite3_stmt * ppStmt = 0;
00754 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00755 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00756 rc = sqlite3_step(ppStmt);
00757 if(rc == SQLITE_ROW)
00758 {
00759 size = sqlite3_column_int64(ppStmt, 0);
00760 rc = sqlite3_step(ppStmt);
00761 }
00762 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00763 rc = sqlite3_finalize(ppStmt);
00764 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00765 }
00766 return size;
00767 }
00768 long DBDriverSqlite3::getUserDataMemoryUsedQuery() const
00769 {
00770 UDEBUG("");
00771 long size = 0L;
00772 if(_ppDb)
00773 {
00774 std::string query;
00775 if(uStrNumCmp(_version, "0.10.1") >= 0)
00776 {
00777 query = "SELECT sum(length(user_data)) from Data;";
00778 }
00779 else if(uStrNumCmp(_version, "0.8.8") >= 0)
00780 {
00781 query = "SELECT sum(length(user_data)) from Node;";
00782 }
00783 else
00784 {
00785 return size;
00786 }
00787
00788 int rc = SQLITE_OK;
00789 sqlite3_stmt * ppStmt = 0;
00790 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00791 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00792 rc = sqlite3_step(ppStmt);
00793 if(rc == SQLITE_ROW)
00794 {
00795 size = sqlite3_column_int64(ppStmt, 0);
00796 rc = sqlite3_step(ppStmt);
00797 }
00798 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00799 rc = sqlite3_finalize(ppStmt);
00800 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00801 }
00802 return size;
00803 }
00804 long DBDriverSqlite3::getWordsMemoryUsedQuery() const
00805 {
00806 UDEBUG("");
00807 long size = 0L;
00808 if(_ppDb)
00809 {
00810 std::string query = "SELECT sum(length(id) + length(descriptor_size) + length(descriptor) + length(time_enter)) from Word;";
00811
00812 int rc = SQLITE_OK;
00813 sqlite3_stmt * ppStmt = 0;
00814 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00815 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00816 rc = sqlite3_step(ppStmt);
00817 if(rc == SQLITE_ROW)
00818 {
00819 size = sqlite3_column_int64(ppStmt, 0);
00820 rc = sqlite3_step(ppStmt);
00821 }
00822 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00823 rc = sqlite3_finalize(ppStmt);
00824 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00825 }
00826 return size;
00827 }
00828 long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
00829 {
00830 UDEBUG("");
00831 long size = 0L;
00832 if(_ppDb)
00833 {
00834 std::string query;
00835 if(uStrNumCmp(_version, "0.13.0") >= 0)
00836 {
00837 query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
00838 "FROM Feature";
00839 }
00840 else if(uStrNumCmp(_version, "0.12.0") >= 0)
00841 {
00842 query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
00843 "FROM Map_Node_Word";
00844 }
00845 else if(uStrNumCmp(_version, "0.11.2") >= 0)
00846 {
00847 query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
00848 "FROM Map_Node_Word";
00849 }
00850 else
00851 {
00852 query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
00853 "FROM Map_Node_Word";
00854 }
00855
00856 int rc = SQLITE_OK;
00857 sqlite3_stmt * ppStmt = 0;
00858 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00859 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00860 rc = sqlite3_step(ppStmt);
00861 if(rc == SQLITE_ROW)
00862 {
00863 size = sqlite3_column_int64(ppStmt, 0);
00864 rc = sqlite3_step(ppStmt);
00865 }
00866 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00867 rc = sqlite3_finalize(ppStmt);
00868 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00869 }
00870 return size;
00871 }
00872 long DBDriverSqlite3::getStatisticsMemoryUsedQuery() const
00873 {
00874 UDEBUG("");
00875 long size = 0L;
00876 if(_ppDb)
00877 {
00878 std::string query;
00879 if(uStrNumCmp(_version, "0.16.2") >= 0)
00880 {
00881 query = "SELECT sum(length(id) + length(stamp) + length(data) + length(wm_state)) FROM Statistics";
00882 }
00883 else if(uStrNumCmp(_version, "0.11.11") >= 0)
00884 {
00885 query = "SELECT sum(length(id) + length(stamp) + length(data)) FROM Statistics";
00886 }
00887 else
00888 {
00889 return size;
00890 }
00891
00892 int rc = SQLITE_OK;
00893 sqlite3_stmt * ppStmt = 0;
00894 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00895 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00896 rc = sqlite3_step(ppStmt);
00897 if(rc == SQLITE_ROW)
00898 {
00899 size = sqlite3_column_int64(ppStmt, 0);
00900 rc = sqlite3_step(ppStmt);
00901 }
00902 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00903 rc = sqlite3_finalize(ppStmt);
00904 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00905 }
00906 return size;
00907 }
00908 int DBDriverSqlite3::getLastNodesSizeQuery() const
00909 {
00910 UDEBUG("");
00911 int size = 0;
00912 if(_ppDb)
00913 {
00914 std::string query;
00915 if(uStrNumCmp(_version, "0.11.11") >= 0)
00916 {
00917 query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
00918 }
00919 else
00920 {
00921 query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
00922 }
00923
00924 int rc = SQLITE_OK;
00925 sqlite3_stmt * ppStmt = 0;
00926 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00927 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00928 rc = sqlite3_step(ppStmt);
00929 if(rc == SQLITE_ROW)
00930 {
00931 size = sqlite3_column_int(ppStmt, 0);
00932 rc = sqlite3_step(ppStmt);
00933 }
00934 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00935 rc = sqlite3_finalize(ppStmt);
00936 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00937 }
00938 return size;
00939 }
00940 int DBDriverSqlite3::getLastDictionarySizeQuery() const
00941 {
00942 UDEBUG("");
00943 int size = 0;
00944 if(_ppDb)
00945 {
00946 std::string query;
00947 if(uStrNumCmp(_version, "0.11.11") >= 0)
00948 {
00949 query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
00950 }
00951 else
00952 {
00953 query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
00954 }
00955
00956 int rc = SQLITE_OK;
00957 sqlite3_stmt * ppStmt = 0;
00958 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00959 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00960 rc = sqlite3_step(ppStmt);
00961 if(rc == SQLITE_ROW)
00962 {
00963 size = sqlite3_column_int(ppStmt, 0);
00964 rc = sqlite3_step(ppStmt);
00965 }
00966 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00967 rc = sqlite3_finalize(ppStmt);
00968 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00969 }
00970 return size;
00971 }
00972 int DBDriverSqlite3::getTotalNodesSizeQuery() const
00973 {
00974 UDEBUG("");
00975 int size = 0;
00976 if(_ppDb)
00977 {
00978 std::string query = "SELECT count(id) from Node;";
00979
00980 int rc = SQLITE_OK;
00981 sqlite3_stmt * ppStmt = 0;
00982 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
00983 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00984 rc = sqlite3_step(ppStmt);
00985 if(rc == SQLITE_ROW)
00986 {
00987 size = sqlite3_column_int(ppStmt, 0);
00988 rc = sqlite3_step(ppStmt);
00989 }
00990 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00991 rc = sqlite3_finalize(ppStmt);
00992 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
00993 }
00994 return size;
00995 }
00996 int DBDriverSqlite3::getTotalDictionarySizeQuery() const
00997 {
00998 UDEBUG("");
00999 int size = 0;
01000 if(_ppDb)
01001 {
01002 std::string query = "SELECT count(id) from Word;";
01003
01004 int rc = SQLITE_OK;
01005 sqlite3_stmt * ppStmt = 0;
01006 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
01007 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01008 rc = sqlite3_step(ppStmt);
01009 if(rc == SQLITE_ROW)
01010 {
01011 size = sqlite3_column_int(ppStmt, 0);
01012 rc = sqlite3_step(ppStmt);
01013 }
01014 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01015 rc = sqlite3_finalize(ppStmt);
01016 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01017 }
01018 return size;
01019 }
01020
01021 ParametersMap DBDriverSqlite3::getLastParametersQuery() const
01022 {
01023 UDEBUG("");
01024 ParametersMap parameters;
01025 if(_ppDb)
01026 {
01027 if(uStrNumCmp(_version, "0.11.8") >= 0)
01028 {
01029 std::string query;
01030 if(uStrNumCmp(_version, "0.11.11") >= 0)
01031 {
01032 query = "SELECT parameters "
01033 "FROM Info "
01034 "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
01035 }
01036 else
01037 {
01038 query = "SELECT parameters "
01039 "FROM Statistics "
01040 "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
01041 }
01042
01043 int rc = SQLITE_OK;
01044 sqlite3_stmt * ppStmt = 0;
01045 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
01046 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01047 rc = sqlite3_step(ppStmt);
01048 if(rc == SQLITE_ROW)
01049 {
01050 std::string text((const char *)sqlite3_column_text(ppStmt, 0));
01051
01052 if(text.size())
01053 {
01054 parameters = Parameters::deserialize(text);
01055 }
01056
01057 rc = sqlite3_step(ppStmt);
01058 }
01059 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01060 rc = sqlite3_finalize(ppStmt);
01061 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01062 }
01063 }
01064 return parameters;
01065 }
01066
01067 std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const
01068 {
01069 UDEBUG("nodeId=%d", nodeId);
01070 std::map<std::string, float> data;
01071 if(_ppDb)
01072 {
01073 if(uStrNumCmp(_version, "0.11.11") >= 0)
01074 {
01075 std::stringstream query;
01076
01077 if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
01078 {
01079 query << "SELECT stamp, data, wm_state "
01080 << "FROM Statistics "
01081 << "WHERE id=" << nodeId << ";";
01082 }
01083 else
01084 {
01085 query << "SELECT stamp, data "
01086 << "FROM Statistics "
01087 << "WHERE id=" << nodeId << ";";
01088 }
01089
01090 int rc = SQLITE_OK;
01091 sqlite3_stmt * ppStmt = 0;
01092 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01093 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01094 rc = sqlite3_step(ppStmt);
01095 if(rc == SQLITE_ROW)
01096 {
01097 int index = 0;
01098 stamp = sqlite3_column_double(ppStmt, index++);
01099
01100 std::string text;
01101 if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
01102 {
01103 const void * dataPtr = sqlite3_column_blob(ppStmt, index);
01104 int dataSize = sqlite3_column_bytes(ppStmt, index++);
01105 if(dataSize>0 && dataPtr)
01106 {
01107 text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
01108 }
01109 }
01110 else
01111 {
01112 text = (const char *)sqlite3_column_text(ppStmt, index++);
01113 }
01114
01115 if(text.size())
01116 {
01117 data = Statistics::deserializeData(text);
01118 }
01119
01120 if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
01121 {
01122 const void * dataPtr = sqlite3_column_blob(ppStmt, index);
01123 int dataSize = sqlite3_column_bytes(ppStmt, index++);
01124 if(dataSize>0 && dataPtr)
01125 {
01126 cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
01127 UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
01128 wmState->resize(wmStateMat.cols);
01129 memcpy(wmState->data(), wmStateMat.data, wmState->size()*sizeof(int));
01130 }
01131 }
01132
01133 rc = sqlite3_step(ppStmt);
01134 }
01135 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01136 rc = sqlite3_finalize(ppStmt);
01137 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01138 }
01139 }
01140 return data;
01141 }
01142
01143 std::map<int, std::pair<std::map<std::string, float>, double> > DBDriverSqlite3::getAllStatisticsQuery() const
01144 {
01145 UDEBUG("");
01146 std::map<int, std::pair<std::map<std::string, float>, double> > data;
01147 if(_ppDb)
01148 {
01149 if(uStrNumCmp(_version, "0.11.11") >= 0)
01150 {
01151 std::stringstream query;
01152
01153 query << "SELECT id, stamp, data "
01154 << "FROM Statistics;";
01155
01156 int rc = SQLITE_OK;
01157 sqlite3_stmt * ppStmt = 0;
01158 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01159 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01160 rc = sqlite3_step(ppStmt);
01161 while(rc == SQLITE_ROW)
01162 {
01163 int index = 0;
01164 int id = sqlite3_column_int(ppStmt, index++);
01165 double stamp = sqlite3_column_double(ppStmt, index++);
01166
01167 std::string text;
01168 if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
01169 {
01170 const void * dataPtr = 0;
01171 int dataSize = 0;
01172 dataPtr = sqlite3_column_blob(ppStmt, index);
01173 dataSize = sqlite3_column_bytes(ppStmt, index++);
01174 if(dataSize>0 && dataPtr)
01175 {
01176 text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
01177 }
01178 }
01179 else
01180 {
01181 text = (const char *)sqlite3_column_text(ppStmt, index++);
01182 }
01183
01184 if(text.size())
01185 {
01186 data.insert(std::make_pair(id, std::make_pair(Statistics::deserializeData(text), stamp)));
01187 }
01188
01189 rc = sqlite3_step(ppStmt);
01190 }
01191 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01192 rc = sqlite3_finalize(ppStmt);
01193 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01194 }
01195 }
01196 UDEBUG("");
01197 return data;
01198 }
01199
01200 std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery() const
01201 {
01202 UDEBUG("");
01203 std::map<int, std::vector<int> > data;
01204 if(_ppDb)
01205 {
01206 if(uStrNumCmp(_version, "0.16.2") >= 0)
01207 {
01208 std::stringstream query;
01209
01210 query << "SELECT id, wm_state "
01211 << "FROM Statistics;";
01212
01213 int rc = SQLITE_OK;
01214 sqlite3_stmt * ppStmt = 0;
01215 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01216 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01217 rc = sqlite3_step(ppStmt);
01218 while(rc == SQLITE_ROW)
01219 {
01220 int index = 0;
01221 int id = sqlite3_column_int(ppStmt, index++);
01222
01223 std::vector<int> wmState;
01224 const void * dataPtr = sqlite3_column_blob(ppStmt, index);
01225 int dataSize = sqlite3_column_bytes(ppStmt, index++);
01226 if(dataSize>0 && dataPtr)
01227 {
01228 cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
01229 UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
01230 wmState.resize(wmStateMat.cols);
01231 memcpy(wmState.data(), wmStateMat.data, wmState.size()*sizeof(int));
01232 }
01233
01234 if(!wmState.empty())
01235 {
01236 data.insert(std::make_pair(id, wmState));
01237 }
01238
01239 rc = sqlite3_step(ppStmt);
01240 }
01241 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01242 rc = sqlite3_finalize(ppStmt);
01243 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01244 }
01245 }
01246 UDEBUG("");
01247 return data;
01248 }
01249
01250 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
01251 {
01252 UDEBUG("load data for %d signatures", (int)signatures.size());
01253
01254 if(!images && !scan && !userData && !occupancyGrid)
01255 {
01256 UWARN("All requested data fields are false! Nothing loaded...");
01257 return;
01258 }
01259
01260 if(_ppDb)
01261 {
01262 UTimer timer;
01263 timer.start();
01264 int rc = SQLITE_OK;
01265 sqlite3_stmt * ppStmt = 0;
01266 std::stringstream query;
01267
01268 if(uStrNumCmp(_version, "0.11.10") >= 0)
01269 {
01270 std::stringstream fields;
01271
01272 if(images)
01273 {
01274 fields << "image, depth, calibration";
01275 if(scan || userData || occupancyGrid)
01276 {
01277 fields << ", ";
01278 }
01279 }
01280 if(scan)
01281 {
01282 fields << "scan_info, scan";
01283 if(userData || occupancyGrid)
01284 {
01285 fields << ", ";
01286 }
01287 }
01288 if(userData)
01289 {
01290 fields << "user_data";
01291 if(occupancyGrid)
01292 {
01293 fields << ", ";
01294 }
01295 }
01296 if(occupancyGrid)
01297 {
01298 if(uStrNumCmp(_version, "0.16.0") >= 0)
01299 {
01300 fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
01301 }
01302 else
01303 {
01304 fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
01305 }
01306 }
01307
01308 query << "SELECT " << fields.str().c_str() << " "
01309 << "FROM Data "
01310 << "WHERE id = ?"
01311 <<";";
01312 }
01313 else if(uStrNumCmp(_version, "0.10.7") >= 0)
01314 {
01315 query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
01316 << "FROM Data "
01317 << "WHERE id = ?"
01318 <<";";
01319 }
01320 else if(uStrNumCmp(_version, "0.10.1") >= 0)
01321 {
01322 query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
01323 << "FROM Data "
01324 << "WHERE id = ?"
01325 <<";";
01326 }
01327 else if(uStrNumCmp(_version, "0.10.0") >= 0)
01328 {
01329 query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data "
01330 << "FROM Data "
01331 << "INNER JOIN Node "
01332 << "ON Data.id = Node.id "
01333 << "WHERE Data.id = ?"
01334 <<";";
01335 }
01336 else if(uStrNumCmp(_version, "0.8.11") >= 0)
01337 {
01338 query << "SELECT Image.data, "
01339 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data "
01340 << "FROM Image "
01341 << "INNER JOIN Node "
01342 << "on Image.id = Node.id "
01343 << "LEFT OUTER JOIN Depth "
01344 << "ON Image.id = Depth.id "
01345 << "WHERE Image.id = ?"
01346 <<";";
01347 }
01348 else if(uStrNumCmp(_version, "0.8.8") >= 0)
01349 {
01350 query << "SELECT Image.data, "
01351 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data "
01352 << "FROM Image "
01353 << "INNER JOIN Node "
01354 << "on Image.id = Node.id "
01355 << "LEFT OUTER JOIN Depth "
01356 << "ON Image.id = Depth.id "
01357 << "WHERE Image.id = ?"
01358 <<";";
01359 }
01360 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01361 {
01362 query << "SELECT Image.data, "
01363 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
01364 << "FROM Image "
01365 << "LEFT OUTER JOIN Depth "
01366 << "ON Image.id = Depth.id "
01367 << "WHERE Image.id = ?"
01368 <<";";
01369 }
01370 else
01371 {
01372 query << "SELECT Image.data, "
01373 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
01374 << "FROM Image "
01375 << "LEFT OUTER JOIN Depth "
01376 << "ON Image.id = Depth.id "
01377 << "WHERE Image.id = ?"
01378 <<";";
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 const void * data = 0;
01385 int dataSize = 0;
01386 int index = 0;
01387
01388 for(std::list<Signature*>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
01389 {
01390 UASSERT(*iter != 0);
01391
01392 ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
01393
01394 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
01395 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01396
01397
01398 rc = sqlite3_step(ppStmt);
01399 if(rc == SQLITE_ROW)
01400 {
01401 index = 0;
01402
01403 cv::Mat imageCompressed;
01404 cv::Mat depthOrRightCompressed;
01405 std::vector<CameraModel> models;
01406 StereoCameraModel stereoModel;
01407 Transform localTransform = Transform::getIdentity();
01408 cv::Mat scanCompressed;
01409 cv::Mat userDataCompressed;
01410
01411 if(uStrNumCmp(_version, "0.11.10") < 0 || images)
01412 {
01413
01414 data = sqlite3_column_blob(ppStmt, index);
01415 dataSize = sqlite3_column_bytes(ppStmt, index++);
01416 if(dataSize>4 && data)
01417 {
01418 imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01419 }
01420
01421
01422 data = sqlite3_column_blob(ppStmt, index);
01423 dataSize = sqlite3_column_bytes(ppStmt, index++);
01424 if(dataSize>4 && data)
01425 {
01426 depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01427 }
01428
01429 if(uStrNumCmp(_version, "0.10.0") < 0)
01430 {
01431 data = sqlite3_column_blob(ppStmt, index);
01432 dataSize = sqlite3_column_bytes(ppStmt, index++);
01433 if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
01434 {
01435 memcpy(localTransform.data(), data, dataSize);
01436 if(uStrNumCmp(_version, "0.15.2") < 0)
01437 {
01438 localTransform.normalizeRotation();
01439 }
01440 }
01441 }
01442
01443
01444 if(uStrNumCmp(_version, "0.10.0") >= 0)
01445 {
01446 data = sqlite3_column_blob(ppStmt, index);
01447 dataSize = sqlite3_column_bytes(ppStmt, index++);
01448
01449
01450 if(dataSize > 0 && data)
01451 {
01452 float * dataFloat = (float*)data;
01453 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
01454 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
01455 {
01456 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
01457 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01458 int max = cameraCount*(6+localTransform.size());
01459 for(int i=0; i<max; i+=6+localTransform.size())
01460 {
01461
01462 localTransform = Transform::getIdentity();
01463 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
01464 if(uStrNumCmp(_version, "0.15.2") < 0)
01465 {
01466 localTransform.normalizeRotation();
01467 }
01468 models.push_back(CameraModel(
01469 (double)dataFloat[i],
01470 (double)dataFloat[i+1],
01471 (double)dataFloat[i+2],
01472 (double)dataFloat[i+3],
01473 localTransform));
01474 models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
01475 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
01476 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
01477 localTransform.prettyPrint().c_str());
01478 }
01479 }
01480 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
01481 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
01482 {
01483 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
01484 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01485 int max = cameraCount*(4+localTransform.size());
01486 for(int i=0; i<max; i+=4+localTransform.size())
01487 {
01488
01489 localTransform = Transform::getIdentity();
01490 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
01491 if(uStrNumCmp(_version, "0.15.2") < 0)
01492 {
01493 localTransform.normalizeRotation();
01494 }
01495 models.push_back(CameraModel(
01496 (double)dataFloat[i],
01497 (double)dataFloat[i+1],
01498 (double)dataFloat[i+2],
01499 (double)dataFloat[i+3],
01500 localTransform));
01501 }
01502 }
01503 else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
01504 {
01505 UDEBUG("Loading calibration of a stereo camera");
01506 memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
01507 if(uStrNumCmp(_version, "0.15.2") < 0)
01508 {
01509 localTransform.normalizeRotation();
01510 }
01511 stereoModel = StereoCameraModel(
01512 dataFloat[0],
01513 dataFloat[1],
01514 dataFloat[2],
01515 dataFloat[3],
01516 dataFloat[4],
01517 localTransform,
01518 cv::Size(dataFloat[5],dataFloat[6]));
01519 }
01520 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
01521 {
01522 UDEBUG("Loading calibration of a stereo camera");
01523 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
01524 if(uStrNumCmp(_version, "0.15.2") < 0)
01525 {
01526 localTransform.normalizeRotation();
01527 }
01528 stereoModel = StereoCameraModel(
01529 dataFloat[0],
01530 dataFloat[1],
01531 dataFloat[2],
01532 dataFloat[3],
01533 dataFloat[4],
01534 localTransform);
01535 }
01536 else
01537 {
01538 UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
01539 }
01540 }
01541
01542 }
01543 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01544 {
01545 UDEBUG("Loading calibration version >= 0.7.0");
01546 double fx = sqlite3_column_double(ppStmt, index++);
01547 double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
01548 double cx = sqlite3_column_double(ppStmt, index++);
01549 double cy = sqlite3_column_double(ppStmt, index++);
01550 if(fyOrBaseline < 1.0)
01551 {
01552
01553 stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
01554 }
01555 else
01556 {
01557 models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
01558 }
01559 }
01560 else
01561 {
01562 UDEBUG("Loading calibration version < 0.7.0");
01563 float depthConstant = sqlite3_column_double(ppStmt, index++);
01564 float fx = 1.0f/depthConstant;
01565 float fy = 1.0f/depthConstant;
01566 float cx = 0.0f;
01567 float cy = 0.0f;
01568 models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
01569 }
01570 }
01571
01572 int laserScanMaxPts = 0;
01573 float laserScanMaxRange = 0.0f;
01574 int laserScanFormat = 0;
01575 Transform scanLocalTransform = Transform::getIdentity();
01576 if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
01577 {
01578
01579 if(uStrNumCmp(_version, "0.11.10") >= 0)
01580 {
01581 data = sqlite3_column_blob(ppStmt, index);
01582 dataSize = sqlite3_column_bytes(ppStmt, index++);
01583
01584 if(dataSize > 0 && data)
01585 {
01586 float * dataFloat = (float*)data;
01587
01588 if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((scanLocalTransform.size()+3)*sizeof(float)))
01589 {
01590
01591 laserScanFormat = (int)dataFloat[2];
01592 memcpy(scanLocalTransform.data(), dataFloat+3, scanLocalTransform.size()*sizeof(float));
01593 }
01594 else if(dataSize == (int)((scanLocalTransform.size()+2)*sizeof(float)))
01595 {
01596 memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
01597 }
01598 else
01599 {
01600 UFATAL("Unexpected size %d for laser scan info!", dataSize);
01601 }
01602
01603 if(uStrNumCmp(_version, "0.15.2") < 0)
01604 {
01605 scanLocalTransform.normalizeRotation();
01606 }
01607 laserScanMaxPts = (int)dataFloat[0];
01608 laserScanMaxRange = dataFloat[1];
01609 }
01610 }
01611 else
01612 {
01613 if(uStrNumCmp(_version, "0.8.11") >= 0)
01614 {
01615 laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
01616 }
01617
01618 if(uStrNumCmp(_version, "0.10.7") >= 0)
01619 {
01620 laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
01621 }
01622 }
01623
01624 data = sqlite3_column_blob(ppStmt, index);
01625 dataSize = sqlite3_column_bytes(ppStmt, index++);
01626
01627 if(dataSize>4 && data)
01628 {
01629 scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01630 }
01631 }
01632
01633 if(uStrNumCmp(_version, "0.11.10") < 0 || userData)
01634 {
01635 if(uStrNumCmp(_version, "0.8.8") >= 0)
01636 {
01637 data = sqlite3_column_blob(ppStmt, index);
01638 dataSize = sqlite3_column_bytes(ppStmt, index++);
01639
01640 if(dataSize>4 && data)
01641 {
01642 if(uStrNumCmp(_version, "0.10.1") >= 0)
01643 {
01644 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
01645 }
01646 else
01647 {
01648
01649 userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
01650 }
01651 }
01652 }
01653 }
01654
01655
01656 cv::Mat groundCellsCompressed;
01657 cv::Mat obstacleCellsCompressed;
01658 cv::Mat emptyCellsCompressed;
01659 float cellSize = 0.0f;
01660 cv::Point3f viewPoint;
01661 if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
01662 {
01663
01664 data = sqlite3_column_blob(ppStmt, index);
01665 dataSize = sqlite3_column_bytes(ppStmt, index++);
01666 if(dataSize > 0 && data)
01667 {
01668 groundCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
01669 memcpy((void*)groundCellsCompressed.data, data, dataSize);
01670 }
01671
01672
01673 data = sqlite3_column_blob(ppStmt, index);
01674 dataSize = sqlite3_column_bytes(ppStmt, index++);
01675 if(dataSize > 0 && data)
01676 {
01677 obstacleCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
01678 memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
01679 }
01680
01681 if(uStrNumCmp(_version, "0.16.0") >= 0)
01682 {
01683
01684 data = sqlite3_column_blob(ppStmt, index);
01685 dataSize = sqlite3_column_bytes(ppStmt, index++);
01686 if(dataSize > 0 && data)
01687 {
01688 emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
01689 memcpy((void*)emptyCellsCompressed.data, data, dataSize);
01690 }
01691 }
01692
01693 cellSize = sqlite3_column_double(ppStmt, index++);
01694 viewPoint.x = sqlite3_column_double(ppStmt, index++);
01695 viewPoint.y = sqlite3_column_double(ppStmt, index++);
01696 viewPoint.z = sqlite3_column_double(ppStmt, index++);
01697 }
01698
01699 SensorData tmp = (*iter)->sensorData();
01700 if(models.size())
01701 {
01702 (*iter)->sensorData() = SensorData(
01703 scan?LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform):tmp.laserScanCompressed(),
01704 images?imageCompressed:tmp.imageCompressed(),
01705 images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
01706 images?models:tmp.cameraModels(),
01707 (*iter)->id(),
01708 (*iter)->getStamp(),
01709 userData?userDataCompressed:tmp.userDataCompressed());
01710 }
01711 else
01712 {
01713 (*iter)->sensorData() = SensorData(
01714 scan?LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform):tmp.laserScanCompressed(),
01715 images?imageCompressed:tmp.imageCompressed(),
01716 images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
01717 images?stereoModel:tmp.stereoCameraModel(),
01718 (*iter)->id(),
01719 (*iter)->getStamp(),
01720 userData?userDataCompressed:tmp.userDataCompressed());
01721 }
01722 if(occupancyGrid)
01723 {
01724 (*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
01725 }
01726 else
01727 {
01728 (*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridEmptyCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
01729 }
01730 rc = sqlite3_step(ppStmt);
01731 }
01732 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01733
01734
01735 rc = sqlite3_reset(ppStmt);
01736 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01737 }
01738
01739
01740 rc = sqlite3_finalize(ppStmt);
01741 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01742 ULOGGER_DEBUG("Time=%fs", timer.ticks());
01743 }
01744 }
01745
01746 bool DBDriverSqlite3::getCalibrationQuery(
01747 int signatureId,
01748 std::vector<CameraModel> & models,
01749 StereoCameraModel & stereoModel) const
01750 {
01751 bool found = false;
01752 if(_ppDb && signatureId)
01753 {
01754 int rc = SQLITE_OK;
01755 sqlite3_stmt * ppStmt = 0;
01756 std::stringstream query;
01757
01758 if(uStrNumCmp(_version, "0.10.0") >= 0)
01759 {
01760 query << "SELECT calibration "
01761 << "FROM Data "
01762 << "WHERE id = " << signatureId
01763 <<";";
01764 }
01765 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01766 {
01767 query << "SELECT local_transform, fx, fy, cx, cy "
01768 << "FROM Depth "
01769 << "WHERE id = " << signatureId
01770 <<";";
01771 }
01772 else
01773 {
01774 query << "SELECT local_transform, constant "
01775 << "FROM Depth "
01776 << "WHERE id = " << signatureId
01777 <<";";
01778 }
01779
01780 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01781 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01782
01783 const void * data = 0;
01784 int dataSize = 0;
01785 Transform localTransform = Transform::getIdentity();
01786
01787
01788 rc = sqlite3_step(ppStmt);
01789 if(rc == SQLITE_ROW)
01790 {
01791 found = true;
01792 int index = 0;
01793
01794
01795 if(uStrNumCmp(_version, "0.10.0") < 0)
01796 {
01797 data = sqlite3_column_blob(ppStmt, index);
01798 dataSize = sqlite3_column_bytes(ppStmt, index++);
01799 if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
01800 {
01801 memcpy(localTransform.data(), data, dataSize);
01802 localTransform.normalizeRotation();
01803 }
01804 }
01805
01806 if(uStrNumCmp(_version, "0.10.0") >= 0)
01807 {
01808 data = sqlite3_column_blob(ppStmt, index);
01809 dataSize = sqlite3_column_bytes(ppStmt, index++);
01810
01811
01812 if(dataSize > 0 && data)
01813 {
01814 float * dataFloat = (float*)data;
01815 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
01816 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
01817 {
01818 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
01819 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01820 int max = cameraCount*(6+localTransform.size());
01821 for(int i=0; i<max; i+=6+localTransform.size())
01822 {
01823
01824 localTransform = Transform::getIdentity();
01825 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
01826 if(uStrNumCmp(_version, "0.15.2") < 0)
01827 {
01828 localTransform.normalizeRotation();
01829 }
01830 models.push_back(CameraModel(
01831 (double)dataFloat[i],
01832 (double)dataFloat[i+1],
01833 (double)dataFloat[i+2],
01834 (double)dataFloat[i+3],
01835 localTransform));
01836 models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
01837 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
01838 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
01839 localTransform.prettyPrint().c_str());
01840 }
01841 }
01842 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
01843 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
01844 {
01845 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
01846 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
01847 int max = cameraCount*(4+localTransform.size());
01848 for(int i=0; i<max; i+=4+localTransform.size())
01849 {
01850
01851 localTransform = Transform::getIdentity();
01852 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
01853 if(uStrNumCmp(_version, "0.15.2") < 0)
01854 {
01855 localTransform.normalizeRotation();
01856 }
01857 models.push_back(CameraModel(
01858 (double)dataFloat[i],
01859 (double)dataFloat[i+1],
01860 (double)dataFloat[i+2],
01861 (double)dataFloat[i+3],
01862 localTransform));
01863 }
01864 }
01865 else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
01866 {
01867 UDEBUG("Loading calibration of a stereo camera");
01868 memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
01869 if(uStrNumCmp(_version, "0.15.2") < 0)
01870 {
01871 localTransform.normalizeRotation();
01872 }
01873 stereoModel = StereoCameraModel(
01874 dataFloat[0],
01875 dataFloat[1],
01876 dataFloat[2],
01877 dataFloat[3],
01878 dataFloat[4],
01879 localTransform,
01880 cv::Size(dataFloat[5],dataFloat[6]));
01881 }
01882 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
01883 {
01884 UDEBUG("Loading calibration of a stereo camera");
01885 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
01886 if(uStrNumCmp(_version, "0.15.2") < 0)
01887 {
01888 localTransform.normalizeRotation();
01889 }
01890 stereoModel = StereoCameraModel(
01891 dataFloat[0],
01892 dataFloat[1],
01893 dataFloat[2],
01894 dataFloat[3],
01895 dataFloat[4],
01896 localTransform);
01897 }
01898 else
01899 {
01900 UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
01901 }
01902 }
01903
01904 }
01905 else if(uStrNumCmp(_version, "0.7.0") >= 0)
01906 {
01907 UDEBUG("Loading calibration version >= 0.7.0");
01908 double fx = sqlite3_column_double(ppStmt, index++);
01909 double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
01910 double cx = sqlite3_column_double(ppStmt, index++);
01911 double cy = sqlite3_column_double(ppStmt, index++);
01912 UDEBUG("fx=%f fyOrBaseline=%f cx=%f cy=%f", fx, fyOrBaseline, cx, cy);
01913 if(fyOrBaseline < 1.0)
01914 {
01915
01916 stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
01917 }
01918 else
01919 {
01920 models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
01921 }
01922 }
01923 else
01924 {
01925 UDEBUG("Loading calibration version < 0.7.0");
01926 float depthConstant = sqlite3_column_double(ppStmt, index++);
01927 float fx = 1.0f/depthConstant;
01928 float fy = 1.0f/depthConstant;
01929 float cx = 0.0f;
01930 float cy = 0.0f;
01931 models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
01932 }
01933
01934 rc = sqlite3_step(ppStmt);
01935 }
01936 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01937
01938
01939 rc = sqlite3_finalize(ppStmt);
01940 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01941 }
01942 return found;
01943 }
01944
01945 bool DBDriverSqlite3::getLaserScanInfoQuery(
01946 int signatureId,
01947 LaserScan & info) const
01948 {
01949 bool found = false;
01950 if(_ppDb && signatureId)
01951 {
01952 int rc = SQLITE_OK;
01953 sqlite3_stmt * ppStmt = 0;
01954 std::stringstream query;
01955
01956 if(uStrNumCmp(_version, "0.11.10") >= 0)
01957 {
01958 query << "SELECT scan_info "
01959 << "FROM Data "
01960 << "WHERE id = " << signatureId
01961 <<";";
01962 }
01963 else
01964 {
01965 return false;
01966 }
01967
01968 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
01969 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
01970
01971 const void * data = 0;
01972 int dataSize = 0;
01973 Transform localTransform = Transform::getIdentity();
01974 int maxPts = 0;
01975 float maxRange = 0.0f;
01976 int format = 0;
01977
01978
01979 rc = sqlite3_step(ppStmt);
01980 if(rc == SQLITE_ROW)
01981 {
01982 found = true;
01983 int index = 0;
01984
01985
01986 data = sqlite3_column_blob(ppStmt, index);
01987 dataSize = sqlite3_column_bytes(ppStmt, index++);
01988
01989 if(dataSize > 0 && data)
01990 {
01991 float * dataFloat = (float*)data;
01992 if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((localTransform.size()+3)*sizeof(float)))
01993 {
01994
01995 format = (int)dataFloat[2];
01996 memcpy(localTransform.data(), dataFloat+3, localTransform.size()*sizeof(float));
01997 }
01998 else if(dataSize == (int)((localTransform.size()+2)*sizeof(float)))
01999 {
02000 memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
02001 }
02002 else
02003 {
02004 UFATAL("Unexpected size %d for laser scan info!", dataSize);
02005 }
02006 if(uStrNumCmp(_version, "0.15.2") < 0)
02007 {
02008 localTransform.normalizeRotation();
02009 }
02010 maxPts = (int)dataFloat[0];
02011 maxRange = dataFloat[1];
02012
02013 info = LaserScan(cv::Mat(), maxPts, maxRange, (LaserScan::Format)format, localTransform);
02014 }
02015
02016 rc = sqlite3_step(ppStmt);
02017 }
02018 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02019
02020
02021 rc = sqlite3_finalize(ppStmt);
02022 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02023 }
02024 return found;
02025 }
02026
02027 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
02028 Transform & pose,
02029 int & mapId,
02030 int & weight,
02031 std::string & label,
02032 double & stamp,
02033 Transform & groundTruthPose,
02034 std::vector<float> & velocity,
02035 GPS & gps) const
02036 {
02037 bool found = false;
02038 if(_ppDb && signatureId)
02039 {
02040 int rc = SQLITE_OK;
02041 sqlite3_stmt * ppStmt = 0;
02042 std::stringstream query;
02043
02044 if(uStrNumCmp(_version, "0.14.0") >= 0)
02045 {
02046 query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps "
02047 "FROM Node "
02048 "WHERE id = " << signatureId <<
02049 ";";
02050 }
02051 else if(uStrNumCmp(_version, "0.13.0") >= 0)
02052 {
02053 query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
02054 "FROM Node "
02055 "WHERE id = " << signatureId <<
02056 ";";
02057 }
02058 else if(uStrNumCmp(_version, "0.11.1") >= 0)
02059 {
02060 query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
02061 "FROM Node "
02062 "WHERE id = " << signatureId <<
02063 ";";
02064 }
02065 else if(uStrNumCmp(_version, "0.8.5") >= 0)
02066 {
02067 query << "SELECT pose, map_id, weight, label, stamp "
02068 "FROM Node "
02069 "WHERE id = " << signatureId <<
02070 ";";
02071 }
02072 else
02073 {
02074 query << "SELECT pose, map_id, weight "
02075 "FROM Node "
02076 "WHERE id = " << signatureId <<
02077 ";";
02078 }
02079
02080 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02081 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02082
02083 const void * data = 0;
02084 int dataSize = 0;
02085
02086
02087 rc = sqlite3_step(ppStmt);
02088 if(rc == SQLITE_ROW)
02089 {
02090 found = true;
02091 int index = 0;
02092 data = sqlite3_column_blob(ppStmt, index);
02093 dataSize = sqlite3_column_bytes(ppStmt, index++);
02094 if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
02095 {
02096 memcpy(pose.data(), data, dataSize);
02097 if(uStrNumCmp(_version, "0.15.2") < 0)
02098 {
02099 pose.normalizeRotation();
02100 }
02101 }
02102
02103 mapId = sqlite3_column_int(ppStmt, index++);
02104 weight = sqlite3_column_int(ppStmt, index++);
02105
02106 if(uStrNumCmp(_version, "0.8.5") >= 0)
02107 {
02108 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
02109 if(p)
02110 {
02111 label = reinterpret_cast<const char*>(p);
02112 }
02113 stamp = sqlite3_column_double(ppStmt, index++);
02114
02115 if(uStrNumCmp(_version, "0.11.1") >= 0)
02116 {
02117 data = sqlite3_column_blob(ppStmt, index);
02118 dataSize = sqlite3_column_bytes(ppStmt, index++);
02119 if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
02120 {
02121 memcpy(groundTruthPose.data(), data, dataSize);
02122 if(uStrNumCmp(_version, "0.15.2") < 0)
02123 {
02124 groundTruthPose.normalizeRotation();
02125 }
02126 }
02127
02128 if(uStrNumCmp(_version, "0.13.0") >= 0)
02129 {
02130 velocity.resize(6,0);
02131 data = sqlite3_column_blob(ppStmt, index);
02132 dataSize = sqlite3_column_bytes(ppStmt, index++);
02133 if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
02134 {
02135 memcpy(velocity.data(), data, dataSize);
02136 }
02137 }
02138
02139 if(uStrNumCmp(_version, "0.14.0") >= 0)
02140 {
02141 std::vector<double> gpsV(6,0);
02142 data = sqlite3_column_blob(ppStmt, index);
02143 dataSize = sqlite3_column_bytes(ppStmt, index++);
02144 if((unsigned int)dataSize == gpsV.size()*sizeof(double) && data)
02145 {
02146 memcpy(gpsV.data(), data, dataSize);
02147 gps = GPS(gpsV[0], gpsV[1], gpsV[2], gpsV[3], gpsV[4], gpsV[5]);
02148 }
02149 }
02150 }
02151 }
02152
02153 rc = sqlite3_step(ppStmt);
02154 }
02155 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02156
02157
02158 rc = sqlite3_finalize(ppStmt);
02159 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02160 }
02161 return found;
02162 }
02163
02164
02165 void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
02166 {
02167 if(_ppDb)
02168 {
02169 UTimer timer;
02170 timer.start();
02171 int rc = SQLITE_OK;
02172 sqlite3_stmt * ppStmt = 0;
02173 std::stringstream query;
02174
02175 query << "SELECT DISTINCT id "
02176 << "FROM Node ";
02177 if(ignoreChildren)
02178 {
02179 query << "INNER JOIN Link "
02180 << "ON id = to_id ";
02181 }
02182 if(ignoreBadSignatures)
02183 {
02184 if(uStrNumCmp(_version, "0.13.0") >= 0)
02185 {
02186 query << "WHERE id in (select node_id from Feature) ";
02187 }
02188 else
02189 {
02190 query << "WHERE id in (select node_id from Map_Node_Word) ";
02191 }
02192 }
02193 query << "ORDER BY id";
02194
02195 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02196 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02197
02198
02199
02200 rc = sqlite3_step(ppStmt);
02201 while(rc == SQLITE_ROW)
02202 {
02203 ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0));
02204 rc = sqlite3_step(ppStmt);
02205 }
02206 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02207
02208
02209 rc = sqlite3_finalize(ppStmt);
02210 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02211 ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
02212 }
02213 }
02214
02215 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const
02216 {
02217 links.clear();
02218 if(_ppDb)
02219 {
02220 UTimer timer;
02221 timer.start();
02222 int rc = SQLITE_OK;
02223 sqlite3_stmt * ppStmt = 0;
02224 std::stringstream query;
02225
02226 if(uStrNumCmp(_version, "0.13.0") >= 0)
02227 {
02228 query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
02229 }
02230 else if(uStrNumCmp(_version, "0.10.10") >= 0)
02231 {
02232 query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
02233 }
02234 else if(uStrNumCmp(_version, "0.8.4") >= 0)
02235 {
02236 query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
02237 }
02238 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02239 {
02240 query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
02241 }
02242 else
02243 {
02244 query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
02245 }
02246
02247 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02248 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02249
02250 int fromId = -1;
02251 int toId = -1;
02252 int type = Link::kUndef;
02253 const void * data = 0;
02254 int dataSize = 0;
02255
02256
02257 rc = sqlite3_step(ppStmt);
02258 while(rc == SQLITE_ROW)
02259 {
02260 int index = 0;
02261
02262 fromId = sqlite3_column_int(ppStmt, index++);
02263 toId = sqlite3_column_int(ppStmt, index++);
02264 type = sqlite3_column_int(ppStmt, index++);
02265
02266 data = sqlite3_column_blob(ppStmt, index);
02267 dataSize = sqlite3_column_bytes(ppStmt, index++);
02268
02269 Transform transform;
02270 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
02271 {
02272 memcpy(transform.data(), data, dataSize);
02273 if(uStrNumCmp(_version, "0.15.2") < 0)
02274 {
02275 transform.normalizeRotation();
02276 }
02277 }
02278 else if(dataSize)
02279 {
02280 UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
02281 }
02282
02283 if(!ignoreNullLinks || !transform.isNull())
02284 {
02285 cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
02286 if(uStrNumCmp(_version, "0.8.4") >= 0)
02287 {
02288 if(uStrNumCmp(_version, "0.13.0") >= 0)
02289 {
02290 data = sqlite3_column_blob(ppStmt, index);
02291 dataSize = sqlite3_column_bytes(ppStmt, index++);
02292 UASSERT(dataSize==36*sizeof(double) && data);
02293 informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone();
02294 }
02295 else
02296 {
02297 double rotVariance = sqlite3_column_double(ppStmt, index++);
02298 double transVariance = sqlite3_column_double(ppStmt, index++);
02299 UASSERT(rotVariance > 0.0 && transVariance>0.0);
02300 informationMatrix.at<double>(0,0) = 1.0/transVariance;
02301 informationMatrix.at<double>(1,1) = 1.0/transVariance;
02302 informationMatrix.at<double>(2,2) = 1.0/transVariance;
02303 informationMatrix.at<double>(3,3) = 1.0/rotVariance;
02304 informationMatrix.at<double>(4,4) = 1.0/rotVariance;
02305 informationMatrix.at<double>(5,5) = 1.0/rotVariance;
02306 }
02307
02308 cv::Mat userDataCompressed;
02309 if(uStrNumCmp(_version, "0.10.10") >= 0)
02310 {
02311 const void * data = sqlite3_column_blob(ppStmt, index);
02312 dataSize = sqlite3_column_bytes(ppStmt, index++);
02313
02314 if(dataSize>4 && data)
02315 {
02316 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
02317 }
02318 }
02319
02320 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
02321 }
02322 else if(uStrNumCmp(_version, "0.7.4") >= 0)
02323 {
02324 double variance = sqlite3_column_double(ppStmt, index++);
02325 UASSERT(variance>0.0);
02326 informationMatrix *= 1.0/variance;
02327 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
02328 }
02329 else
02330 {
02331
02332 links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
02333 }
02334 }
02335
02336 rc = sqlite3_step(ppStmt);
02337 }
02338
02339 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02340
02341
02342 rc = sqlite3_finalize(ppStmt);
02343 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02344 }
02345 }
02346
02347 void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id) const
02348 {
02349 if(_ppDb)
02350 {
02351 UDEBUG("get last id from table \"%s\"", tableName.c_str());
02352 UTimer timer;
02353 timer.start();
02354 int rc = SQLITE_OK;
02355 sqlite3_stmt * ppStmt = 0;
02356 std::stringstream query;
02357
02358 query << "SELECT max(id) "
02359 << "FROM " << tableName
02360 << ";";
02361
02362 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02363 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02364
02365
02366
02367 rc = sqlite3_step(ppStmt);
02368 if(rc == SQLITE_ROW)
02369 {
02370 id = sqlite3_column_int(ppStmt, 0);
02371 rc = sqlite3_step(ppStmt);
02372 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02373 }
02374 else
02375 {
02376 ULOGGER_ERROR("No result !?! from the DB");
02377 }
02378
02379
02380 rc = sqlite3_finalize(ppStmt);
02381 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02382 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02383 }
02384 }
02385
02386 void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
02387 {
02388 ni = 0;
02389 if(_ppDb)
02390 {
02391 UTimer timer;
02392 timer.start();
02393 int rc = SQLITE_OK;
02394 sqlite3_stmt * ppStmt = 0;
02395 std::stringstream query;
02396
02397 if(uStrNumCmp(_version, "0.13.0") >= 0)
02398 {
02399 query << "SELECT count(word_id) "
02400 << "FROM Feature "
02401 << "WHERE node_id=" << nodeId << ";";
02402 }
02403 else
02404 {
02405 query << "SELECT count(word_id) "
02406 << "FROM Map_Node_Word "
02407 << "WHERE node_id=" << nodeId << ";";
02408 }
02409
02410 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02411 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02412
02413
02414
02415 rc = sqlite3_step(ppStmt);
02416 if(rc == SQLITE_ROW)
02417 {
02418 ni = sqlite3_column_int(ppStmt, 0);
02419 rc = sqlite3_step(ppStmt);
02420 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02421 }
02422 else
02423 {
02424 ULOGGER_ERROR("No result !?! from the DB, node=%d",nodeId);
02425 }
02426
02427
02428
02429 rc = sqlite3_finalize(ppStmt);
02430 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02431 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02432 }
02433 }
02434
02435 void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
02436 {
02437 if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
02438 {
02439 UTimer timer;
02440 timer.start();
02441 int rc = SQLITE_OK;
02442 sqlite3_stmt * ppStmt = 0;
02443 std::stringstream query;
02444 query << "SELECT id FROM Node WHERE label='" << label <<"'";
02445
02446 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02447 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02448
02449
02450 rc = sqlite3_step(ppStmt);
02451 if(rc == SQLITE_ROW)
02452 {
02453 id = sqlite3_column_int(ppStmt, 0);
02454 rc = sqlite3_step(ppStmt);
02455 }
02456 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02457
02458
02459 rc = sqlite3_finalize(ppStmt);
02460 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02461 ULOGGER_DEBUG("Time=%f", timer.ticks());
02462 }
02463 }
02464
02465 void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
02466 {
02467 if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
02468 {
02469 UTimer timer;
02470 timer.start();
02471 int rc = SQLITE_OK;
02472 sqlite3_stmt * ppStmt = 0;
02473 std::stringstream query;
02474 query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
02475
02476 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02477 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02478
02479
02480 rc = sqlite3_step(ppStmt);
02481 while(rc == SQLITE_ROW)
02482 {
02483 int index = 0;
02484 int id = sqlite3_column_int(ppStmt, index++);
02485 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
02486 if(p)
02487 {
02488 std::string label = reinterpret_cast<const char*>(p);
02489 if(!label.empty())
02490 {
02491 labels.insert(std::make_pair(id, label));
02492 }
02493 }
02494 rc = sqlite3_step(ppStmt);
02495 }
02496 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02497
02498
02499 rc = sqlite3_finalize(ppStmt);
02500 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02501 ULOGGER_DEBUG("Time=%f", timer.ticks());
02502 }
02503 }
02504
02505 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
02506 {
02507 weight = 0;
02508 if(_ppDb)
02509 {
02510 UTimer timer;
02511 timer.start();
02512 int rc = SQLITE_OK;
02513 sqlite3_stmt * ppStmt = 0;
02514 std::stringstream query;
02515
02516 query << "SELECT weight FROM node WHERE id = "
02517 << nodeId
02518 << ";";
02519
02520 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02521 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02522
02523
02524
02525 rc = sqlite3_step(ppStmt);
02526 if(rc == SQLITE_ROW)
02527 {
02528 weight= sqlite3_column_int(ppStmt, 0);
02529 rc = sqlite3_step(ppStmt);
02530 }
02531 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02532
02533
02534 rc = sqlite3_finalize(ppStmt);
02535 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02536 }
02537 }
02538
02539
02540 void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
02541 {
02542 ULOGGER_DEBUG("count=%d", (int)ids.size());
02543 if(_ppDb && ids.size())
02544 {
02545 std::string type;
02546 UTimer timer;
02547 timer.start();
02548 int rc = SQLITE_OK;
02549 sqlite3_stmt * ppStmt = 0;
02550 std::stringstream query;
02551 unsigned int loaded = 0;
02552
02553
02554 if(uStrNumCmp(_version, "0.14.0") >= 0)
02555 {
02556 query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps "
02557 << "FROM Node "
02558 << "WHERE id=?;";
02559 }
02560 else if(uStrNumCmp(_version, "0.13.0") >= 0)
02561 {
02562 query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
02563 << "FROM Node "
02564 << "WHERE id=?;";
02565 }
02566 else if(uStrNumCmp(_version, "0.11.1") >= 0)
02567 {
02568 query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
02569 << "FROM Node "
02570 << "WHERE id=?;";
02571 }
02572 else if(uStrNumCmp(_version, "0.8.5") >= 0)
02573 {
02574 query << "SELECT id, map_id, weight, pose, stamp, label "
02575 << "FROM Node "
02576 << "WHERE id=?;";
02577 }
02578 else
02579 {
02580 query << "SELECT id, map_id, weight, pose "
02581 << "FROM Node "
02582 << "WHERE id=?;";
02583 }
02584
02585 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
02586 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02587
02588 for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
02589 {
02590
02591
02592 rc = sqlite3_bind_int(ppStmt, 1, *iter);
02593 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02594
02595 int id = 0;
02596 int mapId = 0;
02597 double stamp = 0.0;
02598 int weight = 0;
02599 Transform pose;
02600 Transform groundTruthPose;
02601 std::vector<float> velocity;
02602 std::vector<double> gps;
02603 const void * data = 0;
02604 int dataSize = 0;
02605 std::string label;
02606
02607
02608 rc = sqlite3_step(ppStmt);
02609 if(rc == SQLITE_ROW)
02610 {
02611 int index = 0;
02612 id = sqlite3_column_int(ppStmt, index++);
02613 mapId = sqlite3_column_int(ppStmt, index++);
02614 weight = sqlite3_column_int(ppStmt, index++);
02615
02616 data = sqlite3_column_blob(ppStmt, index);
02617 dataSize = sqlite3_column_bytes(ppStmt, index++);
02618 if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
02619 {
02620 memcpy(pose.data(), data, dataSize);
02621 if(uStrNumCmp(_version, "0.15.2") < 0)
02622 {
02623 pose.normalizeRotation();
02624 }
02625 }
02626
02627 if(uStrNumCmp(_version, "0.8.5") >= 0)
02628 {
02629 stamp = sqlite3_column_double(ppStmt, index++);
02630 const unsigned char * p = sqlite3_column_text(ppStmt, index++);
02631 if(p)
02632 {
02633 label = reinterpret_cast<const char*>(p);
02634 }
02635
02636 if(uStrNumCmp(_version, "0.11.1") >= 0)
02637 {
02638 data = sqlite3_column_blob(ppStmt, index);
02639 dataSize = sqlite3_column_bytes(ppStmt, index++);
02640 if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
02641 {
02642 memcpy(groundTruthPose.data(), data, dataSize);
02643 if(uStrNumCmp(_version, "0.15.2") < 0)
02644 {
02645 groundTruthPose.normalizeRotation();
02646 }
02647 }
02648
02649 if(uStrNumCmp(_version, "0.13.0") >= 0)
02650 {
02651 velocity.resize(6,0);
02652 data = sqlite3_column_blob(ppStmt, index);
02653 dataSize = sqlite3_column_bytes(ppStmt, index++);
02654 if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
02655 {
02656 memcpy(velocity.data(), data, dataSize);
02657 }
02658 }
02659
02660 if(uStrNumCmp(_version, "0.14.0") >= 0)
02661 {
02662 gps.resize(6,0);
02663 data = sqlite3_column_blob(ppStmt, index);
02664 dataSize = sqlite3_column_bytes(ppStmt, index++);
02665 if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
02666 {
02667 memcpy(gps.data(), data, dataSize);
02668 }
02669 }
02670 }
02671 }
02672
02673 rc = sqlite3_step(ppStmt);
02674 }
02675 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02676
02677
02678 if(id)
02679 {
02680 ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
02681 Signature * s = new Signature(
02682 id,
02683 mapId,
02684 weight,
02685 stamp,
02686 label,
02687 pose,
02688 groundTruthPose);
02689 if(velocity.size() == 6)
02690 {
02691 s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
02692 }
02693 if(gps.size() == 6)
02694 {
02695 s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
02696 }
02697 s->setSaved(true);
02698 nodes.push_back(s);
02699 ++loaded;
02700 }
02701 else
02702 {
02703 UERROR("Signature %d not found in database!", *iter);
02704 }
02705
02706
02707 rc = sqlite3_reset(ppStmt);
02708 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02709 }
02710
02711
02712 rc = sqlite3_finalize(ppStmt);
02713 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02714
02715 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02716
02717
02718 std::stringstream query2;
02719 if(uStrNumCmp(_version, "0.13.0") >= 0)
02720 {
02721 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
02722 "FROM Feature "
02723 "WHERE node_id = ? ";
02724 }
02725 else if(uStrNumCmp(_version, "0.12.0") >= 0)
02726 {
02727 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
02728 "FROM Map_Node_Word "
02729 "WHERE node_id = ? ";
02730 }
02731 else if(uStrNumCmp(_version, "0.11.2") >= 0)
02732 {
02733 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
02734 "FROM Map_Node_Word "
02735 "WHERE node_id = ? ";
02736 }
02737 else
02738 {
02739 query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
02740 "FROM Map_Node_Word "
02741 "WHERE node_id = ? ";
02742 }
02743
02744 query2 << " ORDER BY word_id";
02745 query2 << ";";
02746
02747 rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
02748 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02749
02750 float nanFloat = std::numeric_limits<float>::quiet_NaN ();
02751
02752 for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
02753 {
02754
02755
02756 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
02757 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02758
02759 int visualWordId = 0;
02760 int descriptorSize = 0;
02761 const void * descriptor = 0;
02762 int dRealSize = 0;
02763 cv::KeyPoint kpt;
02764 std::multimap<int, cv::KeyPoint> visualWords;
02765 std::multimap<int, cv::Point3f> visualWords3;
02766 std::multimap<int, cv::Mat> descriptors;
02767 cv::Point3f depth(0,0,0);
02768
02769
02770 rc = sqlite3_step(ppStmt);
02771 while(rc == SQLITE_ROW)
02772 {
02773 int index = 0;
02774 visualWordId = sqlite3_column_int(ppStmt, index++);
02775 kpt.pt.x = sqlite3_column_double(ppStmt, index++);
02776 kpt.pt.y = sqlite3_column_double(ppStmt, index++);
02777 kpt.size = sqlite3_column_int(ppStmt, index++);
02778 kpt.angle = sqlite3_column_double(ppStmt, index++);
02779 kpt.response = sqlite3_column_double(ppStmt, index++);
02780 if(uStrNumCmp(_version, "0.12.0") >= 0)
02781 {
02782 kpt.octave = sqlite3_column_int(ppStmt, index++);
02783 }
02784
02785 if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
02786 {
02787 depth.x = nanFloat;
02788 ++index;
02789 }
02790 else
02791 {
02792 depth.x = sqlite3_column_double(ppStmt, index++);
02793 }
02794
02795 if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
02796 {
02797 depth.y = nanFloat;
02798 ++index;
02799 }
02800 else
02801 {
02802 depth.y = sqlite3_column_double(ppStmt, index++);
02803 }
02804
02805 if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
02806 {
02807 depth.z = nanFloat;
02808 ++index;
02809 }
02810 else
02811 {
02812 depth.z = sqlite3_column_double(ppStmt, index++);
02813 }
02814
02815 visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
02816 visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
02817
02818 if(uStrNumCmp(_version, "0.11.2") >= 0)
02819 {
02820 descriptorSize = sqlite3_column_int(ppStmt, index++);
02821 descriptor = sqlite3_column_blob(ppStmt, index);
02822 dRealSize = sqlite3_column_bytes(ppStmt, index++);
02823
02824 if(descriptor && descriptorSize>0 && dRealSize>0)
02825 {
02826 cv::Mat d;
02827 if(dRealSize == descriptorSize)
02828 {
02829
02830 d = cv::Mat(1, descriptorSize, CV_8U);
02831 }
02832 else if(dRealSize/int(sizeof(float)) == descriptorSize)
02833 {
02834
02835 d = cv::Mat(1, descriptorSize, CV_32F);
02836 }
02837 else
02838 {
02839 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
02840 }
02841
02842 memcpy(d.data, descriptor, dRealSize);
02843
02844 descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
02845 }
02846 }
02847
02848 rc = sqlite3_step(ppStmt);
02849 }
02850 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02851
02852 if(visualWords.size()==0)
02853 {
02854 UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
02855 }
02856 else
02857 {
02858 (*iter)->setWords(visualWords);
02859 (*iter)->setWords3(visualWords3);
02860 (*iter)->setWordsDescriptors(descriptors);
02861 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());
02862 }
02863
02864
02865 rc = sqlite3_reset(ppStmt);
02866 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02867 }
02868
02869
02870 rc = sqlite3_finalize(ppStmt);
02871 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02872
02873 ULOGGER_DEBUG("Time=%fs", timer.ticks());
02874
02875 this->loadLinksQuery(nodes);
02876 for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
02877 {
02878 (*iter)->setModified(false);
02879 }
02880 ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
02881
02882
02883 if(nodes.size() && uStrNumCmp(_version, "0.10.0") >= 0)
02884 {
02885 std::stringstream query3;
02886 query3 << "SELECT calibration "
02887 "FROM Data "
02888 "WHERE id = ? ";
02889
02890 rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
02891 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02892
02893 for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
02894 {
02895
02896 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
02897 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
02898
02899 rc = sqlite3_step(ppStmt);
02900 if(rc == SQLITE_ROW)
02901 {
02902 int index=0;
02903 const void * data = 0;
02904 int dataSize = 0;
02905 Transform localTransform;
02906 std::vector<CameraModel> models;
02907 StereoCameraModel stereoModel;
02908
02909
02910 data = sqlite3_column_blob(ppStmt, index);
02911 dataSize = sqlite3_column_bytes(ppStmt, index++);
02912
02913
02914 if(dataSize > 0 && data)
02915 {
02916 float * dataFloat = (float*)data;
02917 if(uStrNumCmp(_version, "0.11.2") >= 0 &&
02918 (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
02919 {
02920 int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
02921 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
02922 int max = cameraCount*(6+localTransform.size());
02923 for(int i=0; i<max; i+=6+localTransform.size())
02924 {
02925
02926 localTransform = Transform::getIdentity();
02927 memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
02928 if(uStrNumCmp(_version, "0.15.2") < 0)
02929 {
02930 localTransform.normalizeRotation();
02931 }
02932 models.push_back(CameraModel(
02933 (double)dataFloat[i],
02934 (double)dataFloat[i+1],
02935 (double)dataFloat[i+2],
02936 (double)dataFloat[i+3],
02937 localTransform,
02938 0,
02939 cv::Size(dataFloat[i+4], dataFloat[i+5])));
02940 UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
02941 dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
02942 localTransform.prettyPrint().c_str());
02943 }
02944 }
02945 else if(uStrNumCmp(_version, "0.11.2") < 0 &&
02946 (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
02947 {
02948 int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
02949 UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
02950 int max = cameraCount*(4+localTransform.size());
02951 for(int i=0; i<max; i+=4+localTransform.size())
02952 {
02953
02954 localTransform = Transform::getIdentity();
02955 memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
02956 if(uStrNumCmp(_version, "0.15.2") < 0)
02957 {
02958 localTransform.normalizeRotation();
02959 }
02960 models.push_back(CameraModel(
02961 (double)dataFloat[i],
02962 (double)dataFloat[i+1],
02963 (double)dataFloat[i+2],
02964 (double)dataFloat[i+3],
02965 localTransform));
02966 }
02967 }
02968 else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
02969 {
02970 UDEBUG("Loading calibration of a stereo camera");
02971 memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
02972 if(uStrNumCmp(_version, "0.15.2") < 0)
02973 {
02974 localTransform.normalizeRotation();
02975 }
02976 stereoModel = StereoCameraModel(
02977 dataFloat[0],
02978 dataFloat[1],
02979 dataFloat[2],
02980 dataFloat[3],
02981 dataFloat[4],
02982 localTransform,
02983 cv::Size(dataFloat[5], dataFloat[6]));
02984 }
02985 else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
02986 {
02987 UDEBUG("Loading calibration of a stereo camera");
02988 memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
02989 if(uStrNumCmp(_version, "0.15.2") < 0)
02990 {
02991 localTransform.normalizeRotation();
02992 }
02993 stereoModel = StereoCameraModel(
02994 dataFloat[0],
02995 dataFloat[1],
02996 dataFloat[2],
02997 dataFloat[3],
02998 dataFloat[4],
02999 localTransform);
03000 }
03001 else
03002 {
03003 UFATAL("Wrong format of the Data.calibration field (size=%d bytes, db version=%s)", dataSize, _version.c_str());
03004 }
03005
03006 (*iter)->sensorData().setCameraModels(models);
03007 (*iter)->sensorData().setStereoCameraModel(stereoModel);
03008 }
03009 rc = sqlite3_step(ppStmt);
03010 }
03011 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03012
03013
03014 rc = sqlite3_reset(ppStmt);
03015 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03016 }
03017
03018 rc = sqlite3_finalize(ppStmt);
03019 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03020
03021 ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
03022 }
03023 if(ids.size() != loaded)
03024 {
03025 UERROR("Some signatures not found in database");
03026 }
03027 }
03028 }
03029
03030 void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
03031 {
03032 ULOGGER_DEBUG("");
03033 if(_ppDb)
03034 {
03035 std::string type;
03036 UTimer timer;
03037 timer.start();
03038 int rc = SQLITE_OK;
03039 sqlite3_stmt * ppStmt = 0;
03040 std::string query;
03041 std::list<int> ids;
03042
03043 if(uStrNumCmp(_version, "0.11.11") >= 0)
03044 {
03045 query = "SELECT n.id "
03046 "FROM Node AS n "
03047 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
03048 "ORDER BY n.id;";
03049 }
03050 else
03051 {
03052 query = "SELECT n.id "
03053 "FROM Node AS n "
03054 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
03055 "ORDER BY n.id;";
03056 }
03057
03058 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03059 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03060
03061
03062 rc = sqlite3_step(ppStmt);
03063 while(rc == SQLITE_ROW)
03064 {
03065 ids.push_back(sqlite3_column_int(ppStmt, 0));
03066 rc = sqlite3_step(ppStmt);
03067 }
03068
03069 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03070
03071
03072 rc = sqlite3_finalize(ppStmt);
03073 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03074
03075 ULOGGER_DEBUG("Loading %d signatures...", ids.size());
03076 this->loadSignaturesQuery(ids, nodes);
03077 ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
03078 }
03079 }
03080
03081 void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
03082 {
03083 ULOGGER_DEBUG("");
03084 if(_ppDb && dictionary)
03085 {
03086 std::string type;
03087 UTimer timer;
03088 timer.start();
03089 int rc = SQLITE_OK;
03090 sqlite3_stmt * ppStmt = 0;
03091 std::stringstream query;
03092 std::list<VisualWord *> visualWords;
03093
03094
03095 query << "SELECT id, descriptor_size, descriptor FROM Word ";
03096 if(lastStateOnly)
03097 {
03098 if(uStrNumCmp(_version, "0.11.11") >= 0)
03099 {
03100 query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
03101 }
03102 else
03103 {
03104 query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
03105 }
03106 }
03107 query << "ORDER BY id;";
03108
03109 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
03110 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03111
03112
03113 int id = 0;
03114 int descriptorSize = 0;
03115 const void * descriptor = 0;
03116 int dRealSize = 0;
03117 rc = sqlite3_step(ppStmt);
03118 int count = 0;
03119 while(rc == SQLITE_ROW)
03120 {
03121 int index=0;
03122 id = sqlite3_column_int(ppStmt, index++);
03123
03124 descriptorSize = sqlite3_column_int(ppStmt, index++);
03125 descriptor = sqlite3_column_blob(ppStmt, index);
03126 dRealSize = sqlite3_column_bytes(ppStmt, index++);
03127
03128 cv::Mat d;
03129 if(dRealSize == descriptorSize)
03130 {
03131
03132 d = cv::Mat(1, descriptorSize, CV_8U);
03133 }
03134 else if(dRealSize/int(sizeof(float)) == descriptorSize)
03135 {
03136
03137 d = cv::Mat(1, descriptorSize, CV_32F);
03138 }
03139 else
03140 {
03141 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
03142 }
03143
03144 memcpy(d.data, descriptor, dRealSize);
03145 VisualWord * vw = new VisualWord(id, d);
03146 vw->setSaved(true);
03147 dictionary->addWord(vw);
03148
03149 if(++count % 5000 == 0)
03150 {
03151 ULOGGER_DEBUG("Loaded %d words...", count);
03152 }
03153 rc = sqlite3_step(ppStmt);
03154 }
03155 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03156
03157 rc = sqlite3_finalize(ppStmt);
03158 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03159
03160
03161 getLastWordId(id);
03162 dictionary->setLastWordId(id);
03163
03164 ULOGGER_DEBUG("Time=%fs", timer.ticks());
03165 }
03166 }
03167
03168
03169 void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
03170 {
03171 ULOGGER_DEBUG("size=%d", wordIds.size());
03172 if(_ppDb && wordIds.size())
03173 {
03174 std::string type;
03175 UTimer timer;
03176 timer.start();
03177 int rc = SQLITE_OK;
03178 sqlite3_stmt * ppStmt = 0;
03179 std::stringstream query;
03180 std::set<int> loaded;
03181
03182
03183 query << "SELECT vw.descriptor_size, vw.descriptor "
03184 "FROM Word as vw "
03185 "WHERE vw.id = ?;";
03186
03187 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
03188 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03189
03190 int descriptorSize;
03191 const void * descriptor;
03192 int dRealSize;
03193 for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
03194 {
03195
03196 rc = sqlite3_bind_int(ppStmt, 1, *iter);
03197 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03198
03199
03200 rc = sqlite3_step(ppStmt);
03201 if(rc == SQLITE_ROW)
03202 {
03203 int index = 0;
03204 descriptorSize = sqlite3_column_int(ppStmt, index++);
03205 descriptor = sqlite3_column_blob(ppStmt, index);
03206 dRealSize = sqlite3_column_bytes(ppStmt, index++);
03207
03208 cv::Mat d;
03209 if(dRealSize == descriptorSize)
03210 {
03211
03212 d = cv::Mat(1, descriptorSize, CV_8U);
03213 }
03214 else if(dRealSize/int(sizeof(float)) == descriptorSize)
03215 {
03216
03217 d = cv::Mat(1, descriptorSize, CV_32F);
03218 }
03219 else
03220 {
03221 UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
03222 }
03223
03224 memcpy(d.data, descriptor, dRealSize);
03225 VisualWord * vw = new VisualWord(*iter, d);
03226 if(vw)
03227 {
03228 vw->setSaved(true);
03229 }
03230 vws.push_back(vw);
03231 loaded.insert(loaded.end(), *iter);
03232
03233 rc = sqlite3_step(ppStmt);
03234 }
03235
03236 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03237
03238 rc = sqlite3_reset(ppStmt);
03239 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03240 }
03241
03242
03243 rc = sqlite3_finalize(ppStmt);
03244 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03245
03246 ULOGGER_DEBUG("Time=%fs", timer.ticks());
03247
03248 if(wordIds.size() != loaded.size())
03249 {
03250 for(std::set<int>::const_iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
03251 {
03252 if(loaded.find(*iter) == loaded.end())
03253 {
03254 UDEBUG("Not found word %d", *iter);
03255 }
03256 }
03257 UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
03258 }
03259 }
03260 }
03261
03262 void DBDriverSqlite3::loadLinksQuery(
03263 int signatureId,
03264 std::map<int, Link> & neighbors,
03265 Link::Type typeIn) const
03266 {
03267 neighbors.clear();
03268 if(_ppDb)
03269 {
03270 UTimer timer;
03271 timer.start();
03272 int rc = SQLITE_OK;
03273 sqlite3_stmt * ppStmt = 0;
03274 std::stringstream query;
03275
03276 if(uStrNumCmp(_version, "0.13.0") >= 0)
03277 {
03278 query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
03279 }
03280 else if(uStrNumCmp(_version, "0.10.10") >= 0)
03281 {
03282 query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
03283 }
03284 else if(uStrNumCmp(_version, "0.8.4") >= 0)
03285 {
03286 query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
03287 }
03288 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03289 {
03290 query << "SELECT to_id, type, transform, variance FROM Link ";
03291 }
03292 else
03293 {
03294 query << "SELECT to_id, type, transform FROM Link ";
03295 }
03296 query << "WHERE from_id = " << signatureId;
03297 if(typeIn != Link::kUndef)
03298 {
03299 if(uStrNumCmp(_version, "0.7.4") >= 0)
03300 {
03301 query << " AND type = " << typeIn;
03302 }
03303 else if(typeIn == Link::kNeighbor)
03304 {
03305 query << " AND type = 0";
03306 }
03307 else if(typeIn > Link::kNeighbor)
03308 {
03309 query << " AND type > 0";
03310 }
03311 }
03312 query << " ORDER BY to_id";
03313
03314 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
03315 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03316
03317 int toId = -1;
03318 int type = Link::kUndef;
03319 const void * data = 0;
03320 int dataSize = 0;
03321
03322
03323 rc = sqlite3_step(ppStmt);
03324 while(rc == SQLITE_ROW)
03325 {
03326 int index = 0;
03327
03328 toId = sqlite3_column_int(ppStmt, index++);
03329 type = sqlite3_column_int(ppStmt, index++);
03330
03331 data = sqlite3_column_blob(ppStmt, index);
03332 dataSize = sqlite3_column_bytes(ppStmt, index++);
03333
03334 Transform transform;
03335 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
03336 {
03337 memcpy(transform.data(), data, dataSize);
03338 if(uStrNumCmp(_version, "0.15.2") < 0)
03339 {
03340 transform.normalizeRotation();
03341 }
03342 }
03343 else if(dataSize)
03344 {
03345 UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
03346 }
03347
03348 cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
03349 if(uStrNumCmp(_version, "0.8.4") >= 0)
03350 {
03351 if(uStrNumCmp(_version, "0.13.0") >= 0)
03352 {
03353 data = sqlite3_column_blob(ppStmt, index);
03354 dataSize = sqlite3_column_bytes(ppStmt, index++);
03355 UASSERT(dataSize==36*sizeof(double) && data);
03356 informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone();
03357 }
03358 else
03359 {
03360 double rotVariance = sqlite3_column_double(ppStmt, index++);
03361 double transVariance = sqlite3_column_double(ppStmt, index++);
03362 UASSERT(rotVariance > 0.0 && transVariance>0.0);
03363 informationMatrix.at<double>(0,0) = 1.0/transVariance;
03364 informationMatrix.at<double>(1,1) = 1.0/transVariance;
03365 informationMatrix.at<double>(2,2) = 1.0/transVariance;
03366 informationMatrix.at<double>(3,3) = 1.0/rotVariance;
03367 informationMatrix.at<double>(4,4) = 1.0/rotVariance;
03368 informationMatrix.at<double>(5,5) = 1.0/rotVariance;
03369 }
03370
03371 cv::Mat userDataCompressed;
03372 if(uStrNumCmp(_version, "0.10.10") >= 0)
03373 {
03374 const void * data = sqlite3_column_blob(ppStmt, index);
03375 dataSize = sqlite3_column_bytes(ppStmt, index++);
03376
03377 if(dataSize>4 && data)
03378 {
03379 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
03380 }
03381 }
03382
03383 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
03384 }
03385 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03386 {
03387 double variance = sqlite3_column_double(ppStmt, index++);
03388 UASSERT(variance>0.0);
03389 informationMatrix *= 1.0/variance;
03390 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
03391 }
03392 else
03393 {
03394
03395 neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
03396 }
03397
03398 rc = sqlite3_step(ppStmt);
03399 }
03400
03401 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03402
03403
03404 rc = sqlite3_finalize(ppStmt);
03405 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03406
03407 if(neighbors.size() == 0)
03408 {
03409
03410 }
03411 }
03412 }
03413
03414 void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
03415 {
03416 if(_ppDb)
03417 {
03418 UTimer timer;
03419 timer.start();
03420 int rc = SQLITE_OK;
03421 sqlite3_stmt * ppStmt = 0;
03422 std::stringstream query;
03423 int totalLinksLoaded = 0;
03424
03425 if(uStrNumCmp(_version, "0.13.0") >= 0)
03426 {
03427 query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
03428 << "WHERE from_id = ? "
03429 << "ORDER BY to_id";
03430 }
03431 else if(uStrNumCmp(_version, "0.10.10") >= 0)
03432 {
03433 query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
03434 << "WHERE from_id = ? "
03435 << "ORDER BY to_id";
03436 }
03437 else if(uStrNumCmp(_version, "0.8.4") >= 0)
03438 {
03439 query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
03440 << "WHERE from_id = ? "
03441 << "ORDER BY to_id";
03442 }
03443 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03444 {
03445 query << "SELECT to_id, type, variance, transform FROM Link "
03446 << "WHERE from_id = ? "
03447 << "ORDER BY to_id";
03448 }
03449 else
03450 {
03451 query << "SELECT to_id, type, transform FROM Link "
03452 << "WHERE from_id = ? "
03453 << "ORDER BY to_id";
03454 }
03455
03456 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
03457 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03458
03459 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
03460 {
03461
03462 rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
03463 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03464
03465 int toId = -1;
03466 int linkType = -1;
03467 std::list<Link> links;
03468 const void * data = 0;
03469 int dataSize = 0;
03470
03471
03472 rc = sqlite3_step(ppStmt);
03473 while(rc == SQLITE_ROW)
03474 {
03475 int index = 0;
03476
03477 toId = sqlite3_column_int(ppStmt, index++);
03478 linkType = sqlite3_column_int(ppStmt, index++);
03479 cv::Mat userDataCompressed;
03480 cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
03481 if(uStrNumCmp(_version, "0.8.4") >= 0)
03482 {
03483 if(uStrNumCmp(_version, "0.13.0") >= 0)
03484 {
03485 data = sqlite3_column_blob(ppStmt, index);
03486 dataSize = sqlite3_column_bytes(ppStmt, index++);
03487 UASSERT(dataSize==36*sizeof(double) && data);
03488 informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone();
03489 }
03490 else
03491 {
03492 double rotVariance = sqlite3_column_double(ppStmt, index++);
03493 double transVariance = sqlite3_column_double(ppStmt, index++);
03494 UASSERT(rotVariance > 0.0 && transVariance>0.0);
03495 informationMatrix.at<double>(0,0) = 1.0/transVariance;
03496 informationMatrix.at<double>(1,1) = 1.0/transVariance;
03497 informationMatrix.at<double>(2,2) = 1.0/transVariance;
03498 informationMatrix.at<double>(3,3) = 1.0/rotVariance;
03499 informationMatrix.at<double>(4,4) = 1.0/rotVariance;
03500 informationMatrix.at<double>(5,5) = 1.0/rotVariance;
03501 }
03502
03503 if(uStrNumCmp(_version, "0.10.10") >= 0)
03504 {
03505 const void * data = sqlite3_column_blob(ppStmt, index);
03506 dataSize = sqlite3_column_bytes(ppStmt, index++);
03507
03508 if(dataSize>4 && data)
03509 {
03510 userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
03511 }
03512 }
03513 }
03514 else if(uStrNumCmp(_version, "0.7.4") >= 0)
03515 {
03516 double variance = sqlite3_column_double(ppStmt, index++);
03517 UASSERT(variance>0.0);
03518 informationMatrix *= 1.0/variance;
03519 }
03520
03521
03522 data = sqlite3_column_blob(ppStmt, index);
03523 dataSize = sqlite3_column_bytes(ppStmt, index++);
03524 Transform transform;
03525 if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
03526 {
03527 memcpy(transform.data(), data, dataSize);
03528 if(uStrNumCmp(_version, "0.15.2") < 0)
03529 {
03530 transform.normalizeRotation();
03531 }
03532 }
03533 else if(dataSize)
03534 {
03535 UERROR("Error while loading link transform from %d to %d! Setting to null...", (*iter)->id(), toId);
03536 }
03537
03538 if(linkType >= 0 && linkType != Link::kUndef)
03539 {
03540 if(uStrNumCmp(_version, "0.7.4") >= 0)
03541 {
03542 links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
03543 }
03544 else
03545 {
03546 links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
03547 }
03548 }
03549 else
03550 {
03551 UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
03552 linkType, (*iter)->id(), toId);
03553 }
03554
03555 ++totalLinksLoaded;
03556 rc = sqlite3_step(ppStmt);
03557 }
03558 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03559
03560
03561 (*iter)->addLinks(links);
03562
03563
03564 rc = sqlite3_reset(ppStmt);
03565 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03566 UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
03567 }
03568
03569
03570 rc = sqlite3_finalize(ppStmt);
03571 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03572 }
03573 }
03574
03575
03576 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
03577 {
03578 UDEBUG("nodes = %d", nodes.size());
03579 if(_ppDb && nodes.size())
03580 {
03581 UTimer timer;
03582 timer.start();
03583 int rc = SQLITE_OK;
03584 sqlite3_stmt * ppStmt = 0;
03585 Signature * s = 0;
03586
03587 std::string query;
03588 if(uStrNumCmp(_version, "0.8.5") >= 0)
03589 {
03590 if(updateTimestamp)
03591 {
03592 query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
03593 }
03594 else
03595 {
03596 query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
03597 }
03598 }
03599 else
03600 {
03601 if(updateTimestamp)
03602 {
03603 query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
03604 }
03605 else
03606 {
03607 query = "UPDATE Node SET weight=? WHERE id=?;";
03608 }
03609 }
03610 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03611 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03612
03613 for(std::list<Signature *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
03614 {
03615 s = *i;
03616 int index = 1;
03617 if(s)
03618 {
03619 rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
03620 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03621
03622 if(uStrNumCmp(_version, "0.8.5") >= 0)
03623 {
03624 if(s->getLabel().empty())
03625 {
03626 rc = sqlite3_bind_null(ppStmt, index++);
03627 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03628 }
03629 else
03630 {
03631 rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
03632 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03633 }
03634 }
03635
03636 rc = sqlite3_bind_int(ppStmt, index++, s->id());
03637 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03638
03639
03640 rc=sqlite3_step(ppStmt);
03641 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03642
03643 rc = sqlite3_reset(ppStmt);
03644 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03645 }
03646 }
03647
03648 rc = sqlite3_finalize(ppStmt);
03649 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03650
03651 ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
03652
03653
03654 query = "DELETE FROM Link WHERE from_id=?;";
03655 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03656 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03657 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
03658 {
03659 if((*j)->isLinksModified())
03660 {
03661 rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
03662 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03663
03664 rc=sqlite3_step(ppStmt);
03665 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03666
03667 rc = sqlite3_reset(ppStmt);
03668 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03669 }
03670 }
03671
03672 rc = sqlite3_finalize(ppStmt);
03673 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03674
03675
03676 query = queryStepLink();
03677 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03678 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03679 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
03680 {
03681 if((*j)->isLinksModified())
03682 {
03683
03684 const std::map<int, Link> & links = (*j)->getLinks();
03685 for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
03686 {
03687 stepLink(ppStmt, i->second);
03688 }
03689 }
03690 }
03691
03692 rc = sqlite3_finalize(ppStmt);
03693 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03694 ULOGGER_DEBUG("Update Neighbors Time=%fs", timer.ticks());
03695
03696
03697 query = queryStepWordsChanged();
03698 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03699 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03700 for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
03701 {
03702 if((*j)->getWordsChanged().size())
03703 {
03704 const std::map<int, int> & wordsChanged = (*j)->getWordsChanged();
03705 for(std::map<int, int>::const_iterator iter=wordsChanged.begin(); iter!=wordsChanged.end(); ++iter)
03706 {
03707 stepWordsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
03708 }
03709 }
03710 }
03711
03712 rc = sqlite3_finalize(ppStmt);
03713 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03714
03715 ULOGGER_DEBUG("signatures update=%fs", timer.ticks());
03716 }
03717 }
03718
03719 void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
03720 {
03721 if(_ppDb && words.size() && updateTimestamp)
03722 {
03723
03724 UTimer timer;
03725 timer.start();
03726 int rc = SQLITE_OK;
03727 sqlite3_stmt * ppStmt = 0;
03728 VisualWord * w = 0;
03729
03730 std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
03731 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03732 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03733
03734 for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
03735 {
03736 w = *i;
03737 int index = 1;
03738 UASSERT(w);
03739
03740 rc = sqlite3_bind_int(ppStmt, index++, w->id());
03741 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03742
03743
03744 rc=sqlite3_step(ppStmt);
03745 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03746
03747 rc = sqlite3_reset(ppStmt);
03748 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03749
03750 }
03751
03752 rc = sqlite3_finalize(ppStmt);
03753 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03754
03755 ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
03756 }
03757 }
03758
03759 void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
03760 {
03761 UDEBUG("");
03762 if(_ppDb && signatures.size())
03763 {
03764 std::string type;
03765 UTimer timer;
03766 timer.start();
03767 int rc = SQLITE_OK;
03768 sqlite3_stmt * ppStmt = 0;
03769
03770
03771 std::string query = queryStepNode();
03772 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03773 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03774
03775 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
03776 {
03777 _memoryUsedEstimate += (*i)->getMemoryUsed();
03778
03779 _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
03780 _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
03781 _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
03782
03783 stepNode(ppStmt, *i);
03784 }
03785
03786 rc = sqlite3_finalize(ppStmt);
03787 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03788
03789 UDEBUG("Time=%fs", timer.ticks());
03790
03791
03792 query = queryStepLink();
03793 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03794 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03795 for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
03796 {
03797
03798 const std::map<int, Link> & links = (*jter)->getLinks();
03799 for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
03800 {
03801 stepLink(ppStmt, i->second);
03802 }
03803 }
03804
03805 rc = sqlite3_finalize(ppStmt);
03806 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03807
03808 UDEBUG("Time=%fs", timer.ticks());
03809
03810
03811
03812 query = queryStepKeypoint();
03813 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03814 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03815 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
03816 {
03817 UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
03818 UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
03819
03820 std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
03821 std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
03822 for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
03823 {
03824 cv::Point3f pt(0,0,0);
03825 if(p!=(*i)->getWords3().end())
03826 {
03827 UASSERT(w->first == p->first);
03828 pt = p->second;
03829 ++p;
03830 }
03831
03832 cv::Mat descriptor;
03833 if(d!=(*i)->getWordsDescriptors().end())
03834 {
03835 UASSERT(w->first == d->first);
03836 descriptor = d->second;
03837 ++d;
03838 }
03839
03840 stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
03841 }
03842 }
03843
03844 rc = sqlite3_finalize(ppStmt);
03845 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03846 UDEBUG("Time=%fs", timer.ticks());
03847
03848 if(uStrNumCmp(_version, "0.10.0") >= 0)
03849 {
03850
03851 query = queryStepSensorData();
03852 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03853 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03854 UDEBUG("Saving %d images", signatures.size());
03855
03856 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
03857 {
03858 if(!(*i)->sensorData().imageCompressed().empty() ||
03859 !(*i)->sensorData().depthOrRightCompressed().empty() ||
03860 !(*i)->sensorData().laserScanCompressed().isEmpty() ||
03861 !(*i)->sensorData().userDataCompressed().empty() ||
03862 !(*i)->sensorData().cameraModels().size() ||
03863 !(*i)->sensorData().stereoCameraModel().isValidForProjection())
03864 {
03865 UASSERT((*i)->id() == (*i)->sensorData().id());
03866 stepSensorData(ppStmt, (*i)->sensorData());
03867 }
03868 }
03869
03870
03871 rc = sqlite3_finalize(ppStmt);
03872 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03873 UDEBUG("Time=%fs", timer.ticks());
03874 }
03875 else
03876 {
03877
03878 query = queryStepImage();
03879 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03880 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03881 UDEBUG("Saving %d images", signatures.size());
03882
03883 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
03884 {
03885 if(!(*i)->sensorData().imageCompressed().empty())
03886 {
03887 stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
03888 }
03889 }
03890
03891
03892 rc = sqlite3_finalize(ppStmt);
03893 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03894 UDEBUG("Time=%fs", timer.ticks());
03895
03896
03897 query = queryStepDepth();
03898 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03899 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03900 for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
03901 {
03902
03903 if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().isEmpty())
03904 {
03905 UASSERT((*i)->id() == (*i)->sensorData().id());
03906 stepDepth(ppStmt, (*i)->sensorData());
03907 }
03908 }
03909
03910 rc = sqlite3_finalize(ppStmt);
03911 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03912 }
03913
03914 UDEBUG("Time=%fs", timer.ticks());
03915 }
03916 }
03917
03918 void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
03919 {
03920 UDEBUG("visualWords size=%d", words.size());
03921 if(_ppDb)
03922 {
03923 std::string type;
03924 UTimer timer;
03925 timer.start();
03926 int rc = SQLITE_OK;
03927 sqlite3_stmt * ppStmt = 0;
03928 std::string query;
03929
03930
03931 if(words.size()>0)
03932 {
03933 query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
03934 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03935 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03936 for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03937 {
03938 const VisualWord * w = *iter;
03939 UASSERT(w);
03940 if(!w->isSaved())
03941 {
03942 rc = sqlite3_bind_int(ppStmt, 1, w->id());
03943 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03944 rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
03945 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03946 UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
03947 if(w->getDescriptor().type() == CV_32F)
03948 {
03949
03950 rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
03951 }
03952 else
03953 {
03954
03955 rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
03956 }
03957 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03958
03959
03960 rc=sqlite3_step(ppStmt);
03961 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
03962
03963 rc = sqlite3_reset(ppStmt);
03964 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03965 }
03966 }
03967
03968 rc = sqlite3_finalize(ppStmt);
03969 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03970 }
03971
03972 UDEBUG("Time=%fs", timer.ticks());
03973 }
03974 }
03975
03976 void DBDriverSqlite3::addLinkQuery(const Link & link) const
03977 {
03978 UDEBUG("");
03979 if(_ppDb)
03980 {
03981 std::string type;
03982 UTimer timer;
03983 timer.start();
03984 int rc = SQLITE_OK;
03985 sqlite3_stmt * ppStmt = 0;
03986
03987
03988 std::string query = queryStepLink();
03989 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
03990 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03991
03992
03993 stepLink(ppStmt, link);
03994
03995
03996 rc = sqlite3_finalize(ppStmt);
03997 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
03998
03999 UDEBUG("Time=%fs", timer.ticks());
04000 }
04001
04002 }
04003
04004 void DBDriverSqlite3::updateLinkQuery(const Link & link) const
04005 {
04006 UDEBUG("");
04007 if(_ppDb)
04008 {
04009 std::string type;
04010 UTimer timer;
04011 timer.start();
04012 int rc = SQLITE_OK;
04013 sqlite3_stmt * ppStmt = 0;
04014
04015
04016 std::string query = queryStepLinkUpdate();
04017 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04018 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04019
04020
04021 stepLink(ppStmt, link);
04022
04023
04024 rc = sqlite3_finalize(ppStmt);
04025 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04026
04027 UDEBUG("Time=%fs", timer.ticks());
04028 }
04029 }
04030
04031 void DBDriverSqlite3::updateOccupancyGridQuery(
04032 int nodeId,
04033 const cv::Mat & ground,
04034 const cv::Mat & obstacles,
04035 const cv::Mat & empty,
04036 float cellSize,
04037 const cv::Point3f & viewpoint) const
04038 {
04039 UDEBUG("");
04040 if(_ppDb)
04041 {
04042 std::string type;
04043 UTimer timer;
04044 timer.start();
04045 int rc = SQLITE_OK;
04046 sqlite3_stmt * ppStmt = 0;
04047
04048
04049 std::string query = queryStepOccupancyGridUpdate();
04050 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04051 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04052
04053
04054 stepOccupancyGridUpdate(ppStmt,
04055 nodeId,
04056 ground,
04057 obstacles,
04058 empty,
04059 cellSize,
04060 viewpoint);
04061
04062
04063 rc = sqlite3_finalize(ppStmt);
04064 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04065
04066 UDEBUG("Time=%fs", timer.ticks());
04067 }
04068 }
04069
04070 void DBDriverSqlite3::updateDepthImageQuery(
04071 int nodeId,
04072 const cv::Mat & image) const
04073 {
04074 UDEBUG("");
04075 if(_ppDb)
04076 {
04077 std::string type;
04078 UTimer timer;
04079 timer.start();
04080 int rc = SQLITE_OK;
04081 sqlite3_stmt * ppStmt = 0;
04082
04083
04084 std::string query = queryStepDepthUpdate();
04085 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04086 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04087
04088
04089 stepDepthUpdate(ppStmt,
04090 nodeId,
04091 image);
04092
04093
04094 rc = sqlite3_finalize(ppStmt);
04095 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04096
04097 UDEBUG("Time=%fs", timer.ticks());
04098 }
04099 }
04100
04101 void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
04102 {
04103 UDEBUG("Ref ID = %d", statistics.refImageId());
04104 if(_ppDb)
04105 {
04106 std::string type;
04107 UTimer timer;
04108 timer.start();
04109 int rc = SQLITE_OK;
04110 sqlite3_stmt * ppStmt = 0;
04111
04112
04113 if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
04114 {
04115 std::string param = Statistics::serializeData(statistics.data());
04116 if(param.size() && statistics.refImageId()>0)
04117 {
04118 std::string query;
04119 if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
04120 {
04121 query = "INSERT INTO Statistics(id, stamp, data, wm_state) values(?,?,?,?);";
04122 }
04123 else
04124 {
04125 query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
04126 }
04127 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04128 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04129
04130 int index = 1;
04131 rc = sqlite3_bind_int(ppStmt, index++, statistics.refImageId());
04132 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04133 rc = sqlite3_bind_double(ppStmt, index++, statistics.stamp());
04134 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04135
04136 cv::Mat compressedParam;
04137 if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
04138 {
04139 compressedParam = compressString(param);
04140 rc = sqlite3_bind_blob(ppStmt, index++, compressedParam.data, compressedParam.cols, SQLITE_STATIC);
04141 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04142 }
04143 else
04144 {
04145 rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, SQLITE_STATIC);
04146 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04147 }
04148
04149 cv::Mat compressedWmState;
04150 if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
04151 {
04152 if(!statistics.wmState().empty())
04153 {
04154 compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
04155 rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
04156 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04157 }
04158 else
04159 {
04160 rc = sqlite3_bind_null(ppStmt, index++);
04161 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04162 }
04163 }
04164
04165
04166 rc=sqlite3_step(ppStmt);
04167 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04168
04169 rc = sqlite3_reset(ppStmt);
04170 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04171
04172
04173 rc = sqlite3_finalize(ppStmt);
04174 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04175
04176 UDEBUG("Time=%fs", timer.ticks());
04177 }
04178 }
04179 }
04180 }
04181
04182 void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
04183 {
04184 UDEBUG("");
04185 if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
04186 {
04187 UTimer timer;
04188 timer.start();
04189 int rc = SQLITE_OK;
04190 sqlite3_stmt * ppStmt = 0;
04191 std::string query;
04192
04193
04194 query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
04195 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04196 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04197
04198 int index = 1;
04199 cv::Mat compressedImage;
04200 if(image.empty())
04201 {
04202 rc = sqlite3_bind_null(ppStmt, index);
04203 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04204 }
04205 else
04206 {
04207
04208 if(image.rows == 1 && image.type() == CV_8UC1)
04209 {
04210
04211 compressedImage = image;
04212 }
04213 else
04214 {
04215 compressedImage = compressImage2(image, ".jpg");
04216 }
04217 rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
04218 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04219 }
04220
04221
04222 rc=sqlite3_step(ppStmt);
04223 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04224
04225
04226 rc = sqlite3_finalize(ppStmt);
04227 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04228
04229 UDEBUG("Time=%fs", timer.ticks());
04230 }
04231 }
04232 cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
04233 {
04234 UDEBUG("");
04235 cv::Mat image;
04236 if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
04237 {
04238 UTimer timer;
04239 timer.start();
04240 int rc = SQLITE_OK;
04241 sqlite3_stmt * ppStmt = 0;
04242 std::stringstream query;
04243
04244 query << "SELECT preview_image "
04245 << "FROM Admin "
04246 << "WHERE version='" << _version.c_str()
04247 <<"';";
04248
04249 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
04250 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04251
04252
04253 rc = sqlite3_step(ppStmt);
04254 UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
04255 if(rc == SQLITE_ROW)
04256 {
04257 const void * data = 0;
04258 int dataSize = 0;
04259 int index = 0;
04260
04261
04262 data = sqlite3_column_blob(ppStmt, index);
04263 dataSize = sqlite3_column_bytes(ppStmt, index++);
04264 if(dataSize>0 && data)
04265 {
04266 image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04267 }
04268 UDEBUG("Image=%dx%d", image.cols, image.rows);
04269
04270 rc = sqlite3_step(ppStmt);
04271 }
04272 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04273
04274
04275 rc = sqlite3_finalize(ppStmt);
04276 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04277 ULOGGER_DEBUG("Time=%fs", timer.ticks());
04278
04279 }
04280 return image;
04281 }
04282
04283 void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
04284 {
04285 UDEBUG("");
04286 if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
04287 {
04288 UTimer timer;
04289 timer.start();
04290 int rc = SQLITE_OK;
04291 sqlite3_stmt * ppStmt = 0;
04292 std::string query;
04293
04294
04295 query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
04296 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04297 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04298
04299 int index = 1;
04300
04301
04302 cv::Mat compressedIds;
04303 cv::Mat compressedPoses;
04304 if(poses.empty())
04305 {
04306 rc = sqlite3_bind_null(ppStmt, index++);
04307 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04308 rc = sqlite3_bind_null(ppStmt, index++);
04309 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04310 }
04311 else
04312 {
04313 std::vector<int> serializedIds(poses.size());
04314 std::vector<float> serializedPoses(poses.size()*12);
04315 int i=0;
04316 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
04317 {
04318 serializedIds[i] = iter->first;
04319 memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
04320 ++i;
04321 }
04322
04323 compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
04324 compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
04325
04326 rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
04327 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04328 rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
04329 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04330 }
04331
04332 if(lastlocalizationPose.isNull())
04333 {
04334 rc = sqlite3_bind_null(ppStmt, index++);
04335 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04336 }
04337 else
04338 {
04339 UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
04340 rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
04341 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04342 }
04343
04344
04345 rc=sqlite3_step(ppStmt);
04346 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04347
04348
04349 rc = sqlite3_finalize(ppStmt);
04350 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04351
04352 UDEBUG("Time=%fs", timer.ticks());
04353 }
04354 }
04355
04356 std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
04357 {
04358 UDEBUG("");
04359 std::map<int, Transform> poses;
04360 if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
04361 {
04362 UTimer timer;
04363 timer.start();
04364 int rc = SQLITE_OK;
04365 sqlite3_stmt * ppStmt = 0;
04366 std::stringstream query;
04367
04368 query << "SELECT opt_ids, opt_poses, opt_last_localization "
04369 << "FROM Admin "
04370 << "WHERE version='" << _version.c_str()
04371 <<"';";
04372
04373 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
04374 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04375
04376
04377 rc = sqlite3_step(ppStmt);
04378 UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
04379 if(rc == SQLITE_ROW)
04380 {
04381 const void * data = 0;
04382 int dataSize = 0;
04383 int index = 0;
04384
04385
04386 cv::Mat serializedIds;
04387 data = sqlite3_column_blob(ppStmt, index);
04388 dataSize = sqlite3_column_bytes(ppStmt, index++);
04389 if(dataSize>0 && data)
04390 {
04391 serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04392 UDEBUG("serializedIds=%d", serializedIds.cols);
04393 }
04394
04395 data = sqlite3_column_blob(ppStmt, index);
04396 dataSize = sqlite3_column_bytes(ppStmt, index++);
04397 if(dataSize>0 && data)
04398 {
04399 cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04400 UDEBUG("serializedPoses=%d", serializedPoses.cols);
04401
04402 UASSERT(serializedIds.cols == serializedPoses.cols/12);
04403 UASSERT(serializedPoses.type() == CV_32FC1);
04404 UASSERT(serializedIds.type() == CV_32SC1);
04405 for(int i=0; i<serializedIds.cols; ++i)
04406 {
04407 Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
04408 serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
04409 serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
04410 poses.insert(std::make_pair(serializedIds.at<int>(i), t));
04411 UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
04412 }
04413 }
04414
04415 data = sqlite3_column_blob(ppStmt, index);
04416 dataSize = sqlite3_column_bytes(ppStmt, index++);
04417 if(lastlocalizationPose)
04418 {
04419 if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
04420 {
04421 memcpy(lastlocalizationPose->data(), data, dataSize);
04422 }
04423 UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
04424 }
04425
04426 rc = sqlite3_step(ppStmt);
04427 }
04428 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04429
04430
04431 rc = sqlite3_finalize(ppStmt);
04432 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04433 ULOGGER_DEBUG("Time=%fs", timer.ticks());
04434
04435 }
04436 return poses;
04437 }
04438
04439 void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
04440 {
04441 UDEBUG("");
04442 if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
04443 {
04444 UTimer timer;
04445 timer.start();
04446 int rc = SQLITE_OK;
04447 sqlite3_stmt * ppStmt = 0;
04448 std::string query;
04449
04450
04451 query = uFormat("UPDATE Admin SET opt_map=?, opt_map_x_min=?, opt_map_y_min=?, opt_map_resolution=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
04452 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04453 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04454
04455 int index = 1;
04456
04457
04458 cv::Mat compressedMap;
04459 if(map.empty())
04460 {
04461 rc = sqlite3_bind_null(ppStmt, index++);
04462 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04463 }
04464 else
04465 {
04466 compressedMap = compressData2(map);
04467
04468 rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
04469 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04470 }
04471
04472 rc = sqlite3_bind_double(ppStmt, index++, xMin);
04473 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04474 rc = sqlite3_bind_double(ppStmt, index++, yMin);
04475 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04476 rc = sqlite3_bind_double(ppStmt, index++, cellSize);
04477 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04478
04479
04480 rc=sqlite3_step(ppStmt);
04481 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04482
04483
04484 rc = sqlite3_finalize(ppStmt);
04485 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04486
04487 UDEBUG("Time=%fs", timer.ticks());
04488 }
04489 }
04490
04491 cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
04492 {
04493 UDEBUG("");
04494 cv::Mat map;
04495 if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
04496 {
04497 UTimer timer;
04498 timer.start();
04499 int rc = SQLITE_OK;
04500 sqlite3_stmt * ppStmt = 0;
04501 std::stringstream query;
04502
04503 query << "SELECT opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
04504 << "FROM Admin "
04505 << "WHERE version='" << _version.c_str()
04506 <<"';";
04507
04508 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
04509 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04510
04511
04512 rc = sqlite3_step(ppStmt);
04513 UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
04514 if(rc == SQLITE_ROW)
04515 {
04516 const void * data = 0;
04517 int dataSize = 0;
04518 int index = 0;
04519
04520
04521 data = sqlite3_column_blob(ppStmt, index);
04522 dataSize = sqlite3_column_bytes(ppStmt, index++);
04523 if(dataSize>0 && data)
04524 {
04525 map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04526 UDEBUG("map=%d/%d", map.cols, map.rows);
04527 }
04528
04529 xMin = sqlite3_column_double(ppStmt, index++);
04530 UDEBUG("xMin=%f", xMin);
04531 yMin = sqlite3_column_double(ppStmt, index++);
04532 UDEBUG("yMin=%f", yMin);
04533 cellSize = sqlite3_column_double(ppStmt, index++);
04534 UDEBUG("cellSize=%f", cellSize);
04535
04536 rc = sqlite3_step(ppStmt);
04537 }
04538 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04539
04540
04541 rc = sqlite3_finalize(ppStmt);
04542 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04543 ULOGGER_DEBUG("Time=%fs", timer.ticks());
04544
04545 }
04546 return map;
04547 }
04548
04549 void DBDriverSqlite3::saveOptimizedMeshQuery(
04550 const cv::Mat & cloud,
04551 const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
04552 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
04553 const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
04554 #else
04555 const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
04556 #endif
04557 const cv::Mat & textures) const
04558 {
04559 UDEBUG("");
04560 if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
04561 {
04562 UTimer timer;
04563 timer.start();
04564 int rc = SQLITE_OK;
04565 sqlite3_stmt * ppStmt = 0;
04566 std::string query;
04567
04568
04569 query = uFormat("UPDATE Admin SET opt_cloud=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
04570 rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
04571 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04572
04573 if(cloud.empty())
04574 {
04575
04576 for(int i=1; i<=5; ++i)
04577 {
04578 rc = sqlite3_bind_null(ppStmt, i);
04579 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04580 }
04581
04582
04583 rc=sqlite3_step(ppStmt);
04584 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04585 }
04586 else
04587 {
04588 int index = 1;
04589
04590
04591 cv::Mat compressedCloud;
04592 if(cloud.rows == 1 && cloud.type() == CV_8UC1)
04593 {
04594
04595 compressedCloud = cloud;
04596 }
04597 else
04598 {
04599 UDEBUG("Cloud points=%d", cloud.cols);
04600 compressedCloud = compressData2(cloud);
04601 }
04602 UDEBUG("Cloud compressed bytes=%d", compressedCloud.cols);
04603 rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
04604 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04605
04606
04607 cv::Mat compressedPolygons;
04608 cv::Mat compressedTexCoords;
04609 cv::Mat compressedTextures;
04610
04611 if(polygons.empty())
04612 {
04613
04614 rc = sqlite3_bind_null(ppStmt, index++);
04615 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04616
04617 rc = sqlite3_bind_null(ppStmt, index++);
04618 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04619
04620 rc = sqlite3_bind_null(ppStmt, index++);
04621 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04622
04623 rc = sqlite3_bind_null(ppStmt, index++);
04624 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04625 }
04626 else
04627 {
04628 std::vector<int> serializedPolygons;
04629 std::vector<float> serializedTexCoords;
04630 int polygonSize = 0;
04631 int totalPolygonIndices = 0;
04632 UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
04633 for(unsigned int t=0; t<polygons.size(); ++t)
04634 {
04635 UDEBUG("t=%d, polygons=%d", t, (int)polygons[t].size());
04636 unsigned int materialPolygonIndices = 0;
04637 for(unsigned int p=0; p<polygons[t].size(); ++p)
04638 {
04639 if(polygonSize == 0)
04640 {
04641 UASSERT(polygons[t][p].size());
04642 polygonSize = polygons[t][p].size();
04643 }
04644 else
04645 {
04646 UASSERT(polygonSize == (int)polygons[t][p].size());
04647 }
04648
04649 materialPolygonIndices += polygons[t][p].size();
04650 }
04651 totalPolygonIndices += materialPolygonIndices;
04652
04653 if(!texCoords.empty())
04654 {
04655 UASSERT(materialPolygonIndices == texCoords[t].size());
04656 }
04657 }
04658 UASSERT(totalPolygonIndices>0);
04659 serializedPolygons.resize(totalPolygonIndices+polygons.size());
04660 if(!texCoords.empty())
04661 {
04662 serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
04663 }
04664
04665 int oi=0;
04666 int ci=0;
04667 for(unsigned int t=0; t<polygons.size(); ++t)
04668 {
04669 serializedPolygons[oi++] = polygons[t].size();
04670 if(!texCoords.empty())
04671 {
04672 serializedTexCoords[ci++] = texCoords[t].size();
04673 }
04674 for(unsigned int p=0; p<polygons[t].size(); ++p)
04675 {
04676 int texIndex = p*polygonSize;
04677 for(unsigned int i=0; i<polygons[t][p].size(); ++i)
04678 {
04679 serializedPolygons[oi++] = polygons[t][p][i];
04680
04681 if(!texCoords.empty())
04682 {
04683 serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
04684 serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
04685 }
04686 }
04687 }
04688 }
04689
04690 UDEBUG("serializedPolygons=%d", (int)serializedPolygons.size());
04691 compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
04692
04693
04694 rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
04695 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04696
04697 rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
04698 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04699
04700
04701 if(texCoords.empty())
04702 {
04703
04704 rc = sqlite3_bind_null(ppStmt, index++);
04705 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04706
04707 rc = sqlite3_bind_null(ppStmt, index++);
04708 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04709 }
04710 else
04711 {
04712 UDEBUG("serializedTexCoords=%d", (int)serializedTexCoords.size());
04713 compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
04714 rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
04715 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04716
04717 UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
04718 if(textures.rows == 1 && textures.type() == CV_8UC1)
04719 {
04720
04721 compressedTextures = textures;
04722 }
04723 else
04724 {
04725 compressedTextures = compressImage2(textures, ".jpg");
04726 }
04727 rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
04728 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04729 }
04730 }
04731
04732
04733 rc=sqlite3_step(ppStmt);
04734 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04735 }
04736
04737
04738 rc = sqlite3_finalize(ppStmt);
04739 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04740
04741 UDEBUG("Time=%fs", timer.ticks());
04742 }
04743 }
04744
04745 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
04746 std::vector<std::vector<std::vector<unsigned int> > > * polygons,
04747 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
04748 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
04749 #else
04750 std::vector<std::vector<Eigen::Vector2f> > * texCoords,
04751 #endif
04752 cv::Mat * textures) const
04753 {
04754 UDEBUG("");
04755 cv::Mat cloud;
04756 if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
04757 {
04758 UTimer timer;
04759 timer.start();
04760 int rc = SQLITE_OK;
04761 sqlite3_stmt * ppStmt = 0;
04762 std::stringstream query;
04763
04764 query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
04765 << "FROM Admin "
04766 << "WHERE version='" << _version.c_str()
04767 <<"';";
04768
04769 rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
04770 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04771
04772
04773 rc = sqlite3_step(ppStmt);
04774 UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
04775 if(rc == SQLITE_ROW)
04776 {
04777 const void * data = 0;
04778 int dataSize = 0;
04779 int index = 0;
04780
04781
04782 data = sqlite3_column_blob(ppStmt, index);
04783 dataSize = sqlite3_column_bytes(ppStmt, index++);
04784 if(dataSize>0 && data)
04785 {
04786 cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04787 }
04788 UDEBUG("Cloud=%d points", cloud.cols);
04789
04790
04791 int polygonSize = sqlite3_column_int(ppStmt, index++);
04792 UDEBUG("polygonSize=%d", polygonSize);
04793
04794
04795 data = sqlite3_column_blob(ppStmt, index);
04796 dataSize = sqlite3_column_bytes(ppStmt, index++);
04797 if(dataSize>0 && data)
04798 {
04799 UASSERT(polygonSize > 0);
04800 if(polygons)
04801 {
04802 cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04803 UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
04804 UASSERT(serializedPolygons.total());
04805 for(int t=0; t<serializedPolygons.cols; ++t)
04806 {
04807 UASSERT(serializedPolygons.at<int>(t) > 0);
04808 std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
04809 ++t;
04810 UASSERT(t < serializedPolygons.cols);
04811 UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
04812 for(int p=0; p<(int)materialPolygons.size(); ++p)
04813 {
04814 for(int i=0; i<polygonSize; ++i)
04815 {
04816 materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
04817 }
04818 }
04819 t+=materialPolygons.size()*polygonSize;
04820 polygons->push_back(materialPolygons);
04821 }
04822 }
04823
04824
04825 data = sqlite3_column_blob(ppStmt, index);
04826 dataSize = sqlite3_column_bytes(ppStmt, index++);
04827 if(dataSize>0 && data)
04828 {
04829 if(texCoords)
04830 {
04831 cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04832 UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
04833 UASSERT(serializedTexCoords.total());
04834 for(int t=0; t<serializedTexCoords.cols; ++t)
04835 {
04836 UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
04837 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
04838 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
04839 #else
04840 std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
04841 #endif
04842 ++t;
04843 UASSERT(t < serializedTexCoords.cols);
04844 UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
04845 for(int p=0; p<(int)materialtexCoords.size(); ++p)
04846 {
04847 materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
04848 materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
04849 }
04850 t+=materialtexCoords.size()*2;
04851 texCoords->push_back(materialtexCoords);
04852 }
04853 }
04854
04855
04856 data = sqlite3_column_blob(ppStmt, index);
04857 dataSize = sqlite3_column_bytes(ppStmt, index++);
04858 if(dataSize>0 && data)
04859 {
04860 if(textures)
04861 {
04862 *textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
04863 UDEBUG("textures=%dx%d", textures->cols, textures->rows);
04864 }
04865 }
04866 }
04867 }
04868
04869 rc = sqlite3_step(ppStmt);
04870 }
04871 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04872
04873
04874 rc = sqlite3_finalize(ppStmt);
04875 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04876 ULOGGER_DEBUG("Time=%fs", timer.ticks());
04877
04878 }
04879 return cloud;
04880 }
04881
04882 std::string DBDriverSqlite3::queryStepNode() const
04883 {
04884 if(uStrNumCmp(_version, "0.14.0") >= 0)
04885 {
04886 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps) VALUES(?,?,?,?,?,?,?,?,?);";
04887 }
04888 else if(uStrNumCmp(_version, "0.13.0") >= 0)
04889 {
04890 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
04891 }
04892 else if(uStrNumCmp(_version, "0.11.1") >= 0)
04893 {
04894 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
04895 }
04896 else if(uStrNumCmp(_version, "0.10.1") >= 0)
04897 {
04898 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
04899 }
04900 else if(uStrNumCmp(_version, "0.8.8") >= 0)
04901 {
04902 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
04903 }
04904 else if(uStrNumCmp(_version, "0.8.5") >= 0)
04905 {
04906 return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
04907 }
04908 return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
04909 }
04910 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
04911 {
04912 UDEBUG("Save node %d", s->id());
04913 if(!ppStmt || !s)
04914 {
04915 UFATAL("");
04916 }
04917 int rc = SQLITE_OK;
04918
04919 int index = 1;
04920 rc = sqlite3_bind_int(ppStmt, index++, s->id());
04921 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04922 rc = sqlite3_bind_int(ppStmt, index++, s->mapId());
04923 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04924 rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
04925 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04926 rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
04927 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04928
04929 if(uStrNumCmp(_version, "0.8.5") >= 0)
04930 {
04931 rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
04932 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04933
04934 if(s->getLabel().empty())
04935 {
04936 rc = sqlite3_bind_null(ppStmt, index++);
04937 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04938 }
04939 else
04940 {
04941 rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
04942 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04943 }
04944 }
04945
04946 std::vector<double> gps;
04947 if(uStrNumCmp(_version, "0.10.1") >= 0)
04948 {
04949
04950
04951 if(uStrNumCmp(_version, "0.11.1") >= 0)
04952 {
04953 rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
04954 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04955
04956 if(uStrNumCmp(_version, "0.13.0") >= 0)
04957 {
04958 if(s->getVelocity().empty())
04959 {
04960 rc = sqlite3_bind_null(ppStmt, index++);
04961 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04962 }
04963 else
04964 {
04965 rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
04966 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04967 }
04968 }
04969
04970 if(uStrNumCmp(_version, "0.14.0") >= 0)
04971 {
04972 if(s->sensorData().gps().stamp() <= 0.0)
04973 {
04974 rc = sqlite3_bind_null(ppStmt, index++);
04975 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04976 }
04977 else
04978 {
04979 gps.resize(6,0.0);
04980 gps[0] = s->sensorData().gps().stamp();
04981 gps[1] = s->sensorData().gps().longitude();
04982 gps[2] = s->sensorData().gps().latitude();
04983 gps[3] = s->sensorData().gps().altitude();
04984 gps[4] = s->sensorData().gps().error();
04985 gps[5] = s->sensorData().gps().bearing();
04986 rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
04987 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04988 }
04989 }
04990 }
04991 }
04992 else if(uStrNumCmp(_version, "0.8.8") >= 0)
04993 {
04994 if(s->sensorData().userDataCompressed().empty())
04995 {
04996 rc = sqlite3_bind_null(ppStmt, index++);
04997 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
04998 }
04999 else
05000 {
05001 rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
05002 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05003 }
05004 }
05005
05006
05007 rc=sqlite3_step(ppStmt);
05008 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05009
05010 rc = sqlite3_reset(ppStmt);
05011 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05012 }
05013
05014 std::string DBDriverSqlite3::queryStepImage() const
05015 {
05016 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
05017 return "INSERT INTO Image(id, data) VALUES(?,?);";
05018 }
05019 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
05020 int id,
05021 const cv::Mat & imageBytes) const
05022 {
05023 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
05024 UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
05025 if(!ppStmt)
05026 {
05027 UFATAL("");
05028 }
05029
05030 int rc = SQLITE_OK;
05031 int index = 1;
05032
05033 rc = sqlite3_bind_int(ppStmt, index++, id);
05034 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05035
05036 if(!imageBytes.empty())
05037 {
05038 rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
05039 }
05040 else
05041 {
05042 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
05043 }
05044 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05045
05046
05047 rc=sqlite3_step(ppStmt);
05048 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05049
05050 rc = sqlite3_reset(ppStmt);
05051 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05052 }
05053
05054 std::string DBDriverSqlite3::queryStepDepth() const
05055 {
05056 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
05057 if(uStrNumCmp(_version, "0.8.11") >= 0)
05058 {
05059 return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
05060 }
05061 else if(uStrNumCmp(_version, "0.7.0") >= 0)
05062 {
05063 return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
05064 }
05065 else
05066 {
05067 return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
05068 }
05069 }
05070 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
05071 {
05072 UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
05073 UDEBUG("Save depth %d (size=%d) depth2d = %d",
05074 sensorData.id(),
05075 (int)sensorData.depthOrRightCompressed().cols,
05076 sensorData.laserScanCompressed().size());
05077 if(!ppStmt)
05078 {
05079 UFATAL("");
05080 }
05081
05082 int rc = SQLITE_OK;
05083 int index = 1;
05084
05085 rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
05086 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05087
05088 if(!sensorData.depthOrRightCompressed().empty())
05089 {
05090 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
05091 }
05092 else
05093 {
05094 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
05095 }
05096 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05097
05098 float fx=0, fyOrBaseline=0, cx=0, cy=0;
05099 Transform localTransform = Transform::getIdentity();
05100 if(sensorData.cameraModels().size())
05101 {
05102 UASSERT_MSG(sensorData.cameraModels().size() == 1,
05103 uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
05104
05105 fx = sensorData.cameraModels()[0].fx();
05106 fyOrBaseline = sensorData.cameraModels()[0].fy();
05107 cx = sensorData.cameraModels()[0].cx();
05108 cy = sensorData.cameraModels()[0].cy();
05109 localTransform = sensorData.cameraModels()[0].localTransform();
05110 }
05111 else if(sensorData.stereoCameraModel().isValidForProjection())
05112 {
05113 fx = sensorData.stereoCameraModel().left().fx();
05114 fyOrBaseline = sensorData.stereoCameraModel().baseline();
05115 cx = sensorData.stereoCameraModel().left().cx();
05116 cy = sensorData.stereoCameraModel().left().cy();
05117 localTransform = sensorData.stereoCameraModel().left().localTransform();
05118 }
05119
05120 if(uStrNumCmp(_version, "0.7.0") >= 0)
05121 {
05122 rc = sqlite3_bind_double(ppStmt, index++, fx);
05123 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05124 rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
05125 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05126 rc = sqlite3_bind_double(ppStmt, index++, cx);
05127 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05128 rc = sqlite3_bind_double(ppStmt, index++, cy);
05129 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05130 }
05131 else
05132 {
05133 rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
05134 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05135 }
05136
05137 rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
05138 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05139
05140 if(!sensorData.laserScanCompressed().isEmpty())
05141 {
05142 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, (int)sensorData.laserScanCompressed().size(), SQLITE_STATIC);
05143 }
05144 else
05145 {
05146 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
05147 }
05148 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05149
05150 if(uStrNumCmp(_version, "0.8.11") >= 0)
05151 {
05152 rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
05153 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05154 }
05155
05156
05157 rc=sqlite3_step(ppStmt);
05158 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05159
05160 rc = sqlite3_reset(ppStmt);
05161 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05162 }
05163
05164 std::string DBDriverSqlite3::queryStepDepthUpdate() const
05165 {
05166 if(uStrNumCmp(_version, "0.10.0") < 0)
05167 {
05168 return "UPDATE Depth SET data=? WHERE id=?;";
05169 }
05170 else
05171 {
05172 return "UPDATE Data SET depth=? WHERE id=?;";
05173 }
05174 }
05175 void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
05176 {
05177 if(!ppStmt)
05178 {
05179 UFATAL("");
05180 }
05181
05182 int rc = SQLITE_OK;
05183 int index = 1;
05184
05185 cv::Mat imageCompressed;
05186 if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
05187 {
05188
05189 imageCompressed = compressImage2(image, ".png");
05190 }
05191 else
05192 {
05193 imageCompressed = image;
05194 }
05195 if(!imageCompressed.empty())
05196 {
05197 rc = sqlite3_bind_blob(ppStmt, index++, imageCompressed.data, (int)imageCompressed.cols, SQLITE_STATIC);
05198 }
05199 else
05200 {
05201 rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
05202 }
05203 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05204
05205
05206 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
05207 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05208
05209
05210 rc=sqlite3_step(ppStmt);
05211 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05212
05213 rc = sqlite3_reset(ppStmt);
05214 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05215 }
05216
05217 std::string DBDriverSqlite3::queryStepSensorData() const
05218 {
05219 UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
05220 if(uStrNumCmp(_version, "0.16.0") >= 0)
05221 {
05222 return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
05223 }
05224 else if(uStrNumCmp(_version, "0.11.10") >= 0)
05225 {
05226 return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
05227 }
05228 else if(uStrNumCmp(_version, "0.10.7") >= 0)
05229 {
05230 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
05231 }
05232 else if(uStrNumCmp(_version, "0.10.1") >= 0)
05233 {
05234 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
05235 }
05236 else
05237 {
05238 return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
05239 }
05240 }
05241 void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
05242 const SensorData & sensorData) const
05243 {
05244 UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
05245 UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
05246 sensorData.id(),
05247 (int)sensorData.imageCompressed().cols,
05248 (int)sensorData.depthOrRightCompressed().cols,
05249 sensorData.laserScanCompressed().size());
05250 if(!ppStmt)
05251 {
05252 UFATAL("");
05253 }
05254
05255 int rc = SQLITE_OK;
05256 int index = 1;
05257
05258
05259 rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
05260 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05261
05262
05263 if(!sensorData.imageCompressed().empty())
05264 {
05265 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
05266 }
05267 else
05268 {
05269 rc = sqlite3_bind_null(ppStmt, index++);
05270 }
05271 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05272
05273
05274 if(!sensorData.depthOrRightCompressed().empty())
05275 {
05276 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
05277 }
05278 else
05279 {
05280 rc = sqlite3_bind_null(ppStmt, index++);
05281 }
05282 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05283
05284
05285 std::vector<float> calibration;
05286
05287
05288 if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
05289 {
05290 if(uStrNumCmp(_version, "0.11.2") >= 0)
05291 {
05292 calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
05293 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
05294 {
05295 UASSERT(sensorData.cameraModels()[i].isValidForProjection());
05296 const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
05297 calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
05298 calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
05299 calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
05300 calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
05301 calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
05302 calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
05303 memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
05304 }
05305 }
05306 else
05307 {
05308 calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
05309 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
05310 {
05311 UASSERT(sensorData.cameraModels()[i].isValidForProjection());
05312 const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
05313 calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
05314 calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
05315 calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
05316 calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
05317 memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
05318 }
05319 }
05320 }
05321 else if(sensorData.stereoCameraModel().isValidForProjection())
05322 {
05323 const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
05324 calibration.resize(7+localTransform.size());
05325 calibration[0] = sensorData.stereoCameraModel().left().fx();
05326 calibration[1] = sensorData.stereoCameraModel().left().fy();
05327 calibration[2] = sensorData.stereoCameraModel().left().cx();
05328 calibration[3] = sensorData.stereoCameraModel().left().cy();
05329 calibration[4] = sensorData.stereoCameraModel().baseline();
05330 calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
05331 calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
05332 memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
05333 }
05334
05335 if(calibration.size())
05336 {
05337 rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
05338 }
05339 else
05340 {
05341 rc = sqlite3_bind_null(ppStmt, index++);
05342 }
05343 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05344
05345 std::vector<float> scanInfo;
05346 if(uStrNumCmp(_version, "0.11.10") >= 0)
05347 {
05348 if(sensorData.laserScanCompressed().maxPoints() > 0 ||
05349 sensorData.laserScanCompressed().maxRange() > 0 ||
05350 (uStrNumCmp(_version, "0.16.1")>=0 && sensorData.laserScanCompressed().format() != LaserScan::kUnknown) ||
05351 (!sensorData.laserScanCompressed().localTransform().isNull() && !sensorData.laserScanCompressed().localTransform().isIdentity()))
05352 {
05353 if(uStrNumCmp(_version, "0.16.1") >=0)
05354 {
05355 scanInfo.resize(3 + Transform().size());
05356 scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
05357 scanInfo[1] = sensorData.laserScanCompressed().maxRange();
05358 scanInfo[2] = sensorData.laserScanCompressed().format();
05359 const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
05360 memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
05361 }
05362 else
05363 {
05364 scanInfo.resize(2 + Transform().size());
05365 scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
05366 scanInfo[1] = sensorData.laserScanCompressed().maxRange();
05367 const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
05368 memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
05369 }
05370 }
05371
05372 if(scanInfo.size())
05373 {
05374 rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
05375 }
05376 else
05377 {
05378 rc = sqlite3_bind_null(ppStmt, index++);
05379 }
05380 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05381
05382 }
05383 else
05384 {
05385
05386 rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
05387 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05388
05389
05390 if(uStrNumCmp(_version, "0.10.7") >= 0)
05391 {
05392 rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanCompressed().maxRange());
05393 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05394 }
05395 }
05396
05397
05398 if(!sensorData.laserScanCompressed().isEmpty())
05399 {
05400 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, sensorData.laserScanCompressed().size(), SQLITE_STATIC);
05401 }
05402 else
05403 {
05404 rc = sqlite3_bind_null(ppStmt, index++);
05405 }
05406 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05407
05408 if(uStrNumCmp(_version, "0.10.1") >= 0)
05409 {
05410
05411 if(!sensorData.userDataCompressed().empty())
05412 {
05413 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
05414 }
05415 else
05416 {
05417 rc = sqlite3_bind_null(ppStmt, index++);
05418 }
05419 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05420 }
05421
05422 if(uStrNumCmp(_version, "0.11.10") >= 0)
05423 {
05424
05425 if(sensorData.gridGroundCellsCompressed().empty())
05426 {
05427 rc = sqlite3_bind_null(ppStmt, index++);
05428 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05429 }
05430 else
05431 {
05432
05433 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
05434 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05435 }
05436
05437
05438 if(sensorData.gridObstacleCellsCompressed().empty())
05439 {
05440 rc = sqlite3_bind_null(ppStmt, index++);
05441 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05442 }
05443 else
05444 {
05445 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
05446 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05447 }
05448
05449 if(uStrNumCmp(_version, "0.16.0") >= 0)
05450 {
05451
05452 if(sensorData.gridEmptyCellsCompressed().empty())
05453 {
05454 rc = sqlite3_bind_null(ppStmt, index++);
05455 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05456 }
05457 else
05458 {
05459 rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
05460 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05461 }
05462 }
05463
05464
05465 rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
05466 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05467
05468
05469 rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
05470 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05471 rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
05472 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05473 rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
05474 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05475 }
05476
05477
05478 rc=sqlite3_step(ppStmt);
05479 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05480
05481 rc = sqlite3_reset(ppStmt);
05482 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05483 }
05484
05485 std::string DBDriverSqlite3::queryStepLinkUpdate() const
05486 {
05487 if(uStrNumCmp(_version, "0.13.0") >= 0)
05488 {
05489 return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
05490 }
05491 else if(uStrNumCmp(_version, "0.10.10") >= 0)
05492 {
05493 return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
05494 }
05495 else if(uStrNumCmp(_version, "0.8.4") >= 0)
05496 {
05497 return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
05498 }
05499 else if(uStrNumCmp(_version, "0.7.4") >= 0)
05500 {
05501 return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
05502 }
05503 else
05504 {
05505 return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
05506 }
05507 }
05508 std::string DBDriverSqlite3::queryStepLink() const
05509 {
05510
05511 if(uStrNumCmp(_version, "0.13.0") >= 0)
05512 {
05513 return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
05514 }
05515 else if(uStrNumCmp(_version, "0.10.10") >= 0)
05516 {
05517 return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
05518 }
05519 else if(uStrNumCmp(_version, "0.8.4") >= 0)
05520 {
05521 return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
05522 }
05523 else if(uStrNumCmp(_version, "0.7.4") >= 0)
05524 {
05525 return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
05526 }
05527 else
05528 {
05529 return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
05530 }
05531 }
05532 void DBDriverSqlite3::stepLink(
05533 sqlite3_stmt * ppStmt,
05534 const Link & link) const
05535 {
05536 if(!ppStmt)
05537 {
05538 UFATAL("");
05539 }
05540 UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
05541
05542
05543 if(link.type()==Link::kVirtualClosure)
05544 {
05545 UDEBUG("Virtual link ignored....");
05546 return;
05547 }
05548
05549 int rc = SQLITE_OK;
05550 int index = 1;
05551 rc = sqlite3_bind_int(ppStmt, index++, link.type());
05552 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05553
05554 if(uStrNumCmp(_version, "0.13.0") >= 0)
05555 {
05556
05557 rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
05558 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05559 }
05560 else if(uStrNumCmp(_version, "0.8.4") >= 0)
05561 {
05562 rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
05563 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05564 rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
05565 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05566 }
05567 else if(uStrNumCmp(_version, "0.7.4") >= 0)
05568 {
05569 rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
05570 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05571 }
05572
05573 rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
05574 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05575
05576 if(uStrNumCmp(_version, "0.10.10") >= 0)
05577 {
05578
05579 if(!link.userDataCompressed().empty())
05580 {
05581 rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
05582 }
05583 else
05584 {
05585 rc = sqlite3_bind_null(ppStmt, index++);
05586 }
05587 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05588 }
05589
05590 rc = sqlite3_bind_int(ppStmt, index++, link.from());
05591 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05592 rc = sqlite3_bind_int(ppStmt, index++, link.to());
05593 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05594
05595 rc=sqlite3_step(ppStmt);
05596 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05597
05598 rc=sqlite3_reset(ppStmt);
05599 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05600 }
05601
05602 std::string DBDriverSqlite3::queryStepWordsChanged() const
05603 {
05604 if(uStrNumCmp(_version, "0.13.0") >= 0)
05605 {
05606 return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
05607 }
05608 else
05609 {
05610 return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
05611 }
05612 }
05613 void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
05614 {
05615 if(!ppStmt)
05616 {
05617 UFATAL("");
05618 }
05619 int rc = SQLITE_OK;
05620 int index = 1;
05621 rc = sqlite3_bind_int(ppStmt, index++, newWordId);
05622 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05623 rc = sqlite3_bind_int(ppStmt, index++, oldWordId);
05624 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05625 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
05626 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05627
05628 rc=sqlite3_step(ppStmt);
05629 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05630
05631 rc=sqlite3_reset(ppStmt);
05632 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05633 }
05634
05635 std::string DBDriverSqlite3::queryStepKeypoint() const
05636 {
05637 if(uStrNumCmp(_version, "0.13.0") >= 0)
05638 {
05639 return "INSERT INTO Feature(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
05640 }
05641 else if(uStrNumCmp(_version, "0.12.0") >= 0)
05642 {
05643 return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
05644 }
05645 else if(uStrNumCmp(_version, "0.11.2") >= 0)
05646 {
05647 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(?,?,?,?,?,?,?,?,?,?,?,?);";
05648 }
05649 return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
05650 }
05651 void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
05652 int nodeId,
05653 int wordId,
05654 const cv::KeyPoint & kp,
05655 const cv::Point3f & pt,
05656 const cv::Mat & descriptor) const
05657 {
05658 if(!ppStmt)
05659 {
05660 UFATAL("");
05661 }
05662 int rc = SQLITE_OK;
05663 int index = 1;
05664 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
05665 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05666 rc = sqlite3_bind_int(ppStmt, index++, wordId);
05667 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05668 rc = sqlite3_bind_double(ppStmt, index++, kp.pt.x);
05669 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05670 rc = sqlite3_bind_double(ppStmt, index++, kp.pt.y);
05671 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05672 rc = sqlite3_bind_int(ppStmt, index++, kp.size);
05673 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05674 rc = sqlite3_bind_double(ppStmt, index++, kp.angle);
05675 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05676 rc = sqlite3_bind_double(ppStmt, index++, kp.response);
05677 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05678 if(uStrNumCmp(_version, "0.12.0") >= 0)
05679 {
05680 rc = sqlite3_bind_int(ppStmt, index++, kp.octave);
05681 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05682 }
05683
05684 if(uIsFinite(pt.x))
05685 {
05686 rc = sqlite3_bind_double(ppStmt, index++, pt.x);
05687 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05688 }
05689 else
05690 {
05691 rc = sqlite3_bind_null(ppStmt, index++);
05692 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05693 }
05694
05695 if(uIsFinite(pt.y))
05696 {
05697 rc = sqlite3_bind_double(ppStmt, index++, pt.y);
05698 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05699 }
05700 else
05701 {
05702 rc = sqlite3_bind_null(ppStmt, index++);
05703 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05704 }
05705
05706 if(uIsFinite(pt.z))
05707 {
05708 rc = sqlite3_bind_double(ppStmt, index++, pt.z);
05709 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05710 }
05711 else
05712 {
05713 rc = sqlite3_bind_null(ppStmt, index++);
05714 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05715 }
05716
05717
05718 if(uStrNumCmp(_version, "0.11.2") >= 0)
05719 {
05720 rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
05721 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05722 UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
05723 if(descriptor.empty())
05724 {
05725 rc = sqlite3_bind_null(ppStmt, index++);
05726 }
05727 else
05728 {
05729 if(descriptor.type() == CV_32F)
05730 {
05731
05732 rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
05733 }
05734 else
05735 {
05736
05737 rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
05738 }
05739 }
05740 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05741 }
05742
05743 rc=sqlite3_step(ppStmt);
05744 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05745
05746 rc = sqlite3_reset(ppStmt);
05747 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05748 }
05749
05750 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
05751 {
05752 UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
05753 if(uStrNumCmp(_version, "0.16.0") >= 0)
05754 {
05755 return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
05756 }
05757 return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
05758 }
05759 void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
05760 int nodeId,
05761 const cv::Mat & ground,
05762 const cv::Mat & obstacles,
05763 const cv::Mat & empty,
05764 float cellSize,
05765 const cv::Point3f & viewpoint) const
05766 {
05767 UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
05768 UASSERT(ground.empty() || ground.type() == CV_8UC1);
05769 UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1);
05770 UASSERT(empty.empty() || empty.type() == CV_8UC1);
05771 UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
05772 nodeId,
05773 ground.cols,
05774 obstacles.cols,
05775 empty.cols,
05776 cellSize,
05777 viewpoint.x,
05778 viewpoint.y,
05779 viewpoint.z);
05780 if(!ppStmt)
05781 {
05782 UFATAL("");
05783 }
05784
05785 int rc = SQLITE_OK;
05786 int index = 1;
05787
05788
05789 if(ground.empty())
05790 {
05791 rc = sqlite3_bind_null(ppStmt, index++);
05792 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05793 }
05794 else
05795 {
05796
05797 rc = sqlite3_bind_blob(ppStmt, index++, ground.data, ground.cols, SQLITE_STATIC);
05798 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05799 }
05800
05801
05802 if(obstacles.empty())
05803 {
05804 rc = sqlite3_bind_null(ppStmt, index++);
05805 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05806 }
05807 else
05808 {
05809 rc = sqlite3_bind_blob(ppStmt, index++, obstacles.data, obstacles.cols, SQLITE_STATIC);
05810 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05811 }
05812
05813 if(uStrNumCmp(_version, "0.16.0") >= 0)
05814 {
05815
05816 if(empty.empty())
05817 {
05818 rc = sqlite3_bind_null(ppStmt, index++);
05819 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05820 }
05821 else
05822 {
05823 rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
05824 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05825 }
05826 }
05827
05828
05829 rc = sqlite3_bind_double(ppStmt, index++, cellSize);
05830 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05831
05832
05833 rc = sqlite3_bind_double(ppStmt, index++, viewpoint.x);
05834 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05835 rc = sqlite3_bind_double(ppStmt, index++, viewpoint.y);
05836 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05837 rc = sqlite3_bind_double(ppStmt, index++, viewpoint.z);
05838 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05839
05840
05841 rc = sqlite3_bind_int(ppStmt, index++, nodeId);
05842 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05843
05844
05845
05846 rc=sqlite3_step(ppStmt);
05847 UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05848
05849 rc = sqlite3_reset(ppStmt);
05850 UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
05851 }
05852
05853 }