DBDriverSqlite3.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/DBDriverSqlite3.h"
29 #include <sqlite3.h>
30 
31 #include "rtabmap/core/Signature.h"
32 #include "rtabmap/core/VisualWord.h"
33 #include "rtabmap/core/VWDictionary.h"
34 #include "rtabmap/core/util3d.h"
35 #include "rtabmap/core/Compression.h"
36 #include "DatabaseSchema_sql.h"
37 #include "DatabaseSchema_0_18_3_sql.h"
38 #include "DatabaseSchema_0_18_0_sql.h"
39 #include "DatabaseSchema_0_17_0_sql.h"
40 #include "DatabaseSchema_0_16_2_sql.h"
41 #include "DatabaseSchema_0_16_1_sql.h"
42 #include "DatabaseSchema_0_16_0_sql.h"
43 
44 
45 #include <set>
46 
47 #include "rtabmap/utilite/UtiLite.h"
48 
49 namespace rtabmap {
50 
51 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
52  DBDriver(parameters),
53  _ppDb(0),
54  _version("0.0.0"),
55  _memoryUsedEstimate(0),
56  _dbInMemory(Parameters::defaultDbSqlite3InMemory()),
57  _cacheSize(Parameters::defaultDbSqlite3CacheSize()),
58  _journalMode(Parameters::defaultDbSqlite3JournalMode()),
59  _synchronous(Parameters::defaultDbSqlite3Synchronous()),
60  _tempStore(Parameters::defaultDbSqlite3TempStore())
61 {
62  ULOGGER_DEBUG("treadSafe=%d", sqlite3_threadsafe());
63  this->parseParameters(parameters);
64 }
65 
66 DBDriverSqlite3::~DBDriverSqlite3()
67 {
68  this->closeConnection();
69 }
70 
71 void DBDriverSqlite3::parseParameters(const ParametersMap & parameters)
72 {
73  ParametersMap::const_iterator iter;
74  if((iter=parameters.find(Parameters::kDbSqlite3CacheSize())) != parameters.end())
75  {
76  this->setCacheSize(std::atoi((*iter).second.c_str()));
77  }
78  if((iter=parameters.find(Parameters::kDbSqlite3JournalMode())) != parameters.end())
79  {
80  this->setJournalMode(std::atoi((*iter).second.c_str()));
81  }
82  if((iter=parameters.find(Parameters::kDbSqlite3Synchronous())) != parameters.end())
83  {
84  this->setSynchronous(std::atoi((*iter).second.c_str()));
85  }
86  if((iter=parameters.find(Parameters::kDbSqlite3TempStore())) != parameters.end())
87  {
88  this->setTempStore(std::atoi((*iter).second.c_str()));
89  }
90  if((iter=parameters.find(Parameters::kDbSqlite3InMemory())) != parameters.end())
91  {
92  this->setDbInMemory(uStr2Bool((*iter).second.c_str()));
93  }
94  DBDriver::parseParameters(parameters);
95 }
96 
97 void DBDriverSqlite3::setCacheSize(unsigned int cacheSize)
98 {
99  if(this->isConnected())
100  {
101  _cacheSize = cacheSize;
102  std::string query = "PRAGMA cache_size = ";
103  query += uNumber2Str(_cacheSize) + ";";
104  this->executeNoResultQuery(query.c_str());
105  }
106 }
107 
108 void DBDriverSqlite3::setJournalMode(int journalMode)
109 {
110  if(journalMode >= 0 && journalMode < 5)
111  {
112  _journalMode = journalMode;
113  if(this->isConnected())
114  {
115  switch(_journalMode)
116  {
117  case 4:
118  this->executeNoResultQuery("PRAGMA journal_mode = OFF;");
119  break;
120  case 3:
121  this->executeNoResultQuery("PRAGMA journal_mode = MEMORY;");
122  break;
123  case 2:
124  this->executeNoResultQuery("PRAGMA journal_mode = PERSIST;");
125  break;
126  case 1:
127  this->executeNoResultQuery("PRAGMA journal_mode = TRUNCATE;");
128  break;
129  case 0:
130  default:
131  this->executeNoResultQuery("PRAGMA journal_mode = DELETE;");
132  break;
133  }
134  }
135  }
136  else
137  {
138  ULOGGER_ERROR("Wrong journal mode (%d)", journalMode);
139  }
140 }
141 
142 void DBDriverSqlite3::setSynchronous(int synchronous)
143 {
144  if(synchronous >= 0 && synchronous < 3)
145  {
146  _synchronous = synchronous;
147  if(this->isConnected())
148  {
149  switch(_synchronous)
150  {
151  case 0:
152  this->executeNoResultQuery("PRAGMA synchronous = OFF;");
153  break;
154  case 1:
155  this->executeNoResultQuery("PRAGMA synchronous = NORMAL;");
156  break;
157  case 2:
158  default:
159  this->executeNoResultQuery("PRAGMA synchronous = FULL;");
160  break;
161  }
162  }
163  }
164  else
165  {
166  ULOGGER_ERROR("Wrong synchronous value (%d)", synchronous);
167  }
168 }
169 
170 void DBDriverSqlite3::setTempStore(int tempStore)
171 {
172  if(tempStore >= 0 && tempStore < 3)
173  {
174  _tempStore = tempStore;
175  if(this->isConnected())
176  {
177  switch(_tempStore)
178  {
179  case 2:
180  this->executeNoResultQuery("PRAGMA temp_store = MEMORY;");
181  break;
182  case 1:
183  this->executeNoResultQuery("PRAGMA temp_store = FILE;");
184  break;
185  case 0:
186  default:
187  this->executeNoResultQuery("PRAGMA temp_store = DEFAULT;");
188  break;
189  }
190  }
191  }
192  else
193  {
194  ULOGGER_ERROR("Wrong tempStore value (%d)", tempStore);
195  }
196 }
197 
198 void DBDriverSqlite3::setDbInMemory(bool dbInMemory)
199 {
200  UDEBUG("dbInMemory=%d", dbInMemory?1:0);
201  if(dbInMemory != _dbInMemory)
202  {
203  if(this->isConnected())
204  {
205  // Hard reset...
206  join(true);
207  this->emptyTrashes();
208  this->closeConnection();
209  _dbInMemory = dbInMemory;
210  this->openConnection(this->getUrl());
211  }
212  else
213  {
214  _dbInMemory = dbInMemory;
215  }
216  }
217 }
218 
219 /*
220 ** This function is used to load the contents of a database file on disk
221 ** into the "main" database of open database connection pInMemory, or
222 ** to save the current contents of the database opened by pInMemory into
223 ** a database file on disk. pInMemory is probably an in-memory database,
224 ** but this function will also work fine if it is not.
225 **
226 ** Parameter zFilename points to a nul-terminated string containing the
227 ** name of the database file on disk to load from or save to. If parameter
228 ** isSave is non-zero, then the contents of the file zFilename are
229 ** overwritten with the contents of the database opened by pInMemory. If
230 ** parameter isSave is zero, then the contents of the database opened by
231 ** pInMemory are replaced by data loaded from the file zFilename.
232 **
233 ** If the operation is successful, SQLITE_OK is returned. Otherwise, if
234 ** an error occurs, an SQLite error code is returned.
235 */
236 int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const
237 {
238  int rc; /* Function return code */
239  sqlite3 *pFile = 0; /* Database connection opened on zFilename */
240  sqlite3_backup *pBackup = 0; /* Backup object used to copy data */
241  sqlite3 *pTo = 0; /* Database to copy to (pFile or pInMemory) */
242  sqlite3 *pFrom = 0; /* Database to copy from (pFile or pInMemory) */
243 
244  /* Open the database file identified by zFilename. Exit early if this fails
245  ** for any reason. */
246  rc = sqlite3_open(fileName.c_str(), &pFile);
247  if( rc==SQLITE_OK ){
248 
249  /* If this is a 'load' operation (isSave==0), then data is copied
250  ** from the database file just opened to database pInMemory.
251  ** Otherwise, if this is a 'save' operation (isSave==1), then data
252  ** is copied from pInMemory to pFile. Set the variables pFrom and
253  ** pTo accordingly. */
254  pFrom = (isSave ? pInMemory : pFile);
255  pTo = (isSave ? pFile : pInMemory);
256 
257  /* Set up the backup procedure to copy from the "main" database of
258  ** connection pFile to the main database of connection pInMemory.
259  ** If something goes wrong, pBackup will be set to NULL and an error
260  ** code and message left in connection pTo.
261  **
262  ** If the backup object is successfully created, call backup_step()
263  ** to copy data from pFile to pInMemory. Then call backup_finish()
264  ** to release resources associated with the pBackup object. If an
265  ** error occurred, then an error code and message will be left in
266  ** connection pTo. If no error occurred, then the error code belonging
267  ** to pTo is set to SQLITE_OK.
268  */
269  pBackup = sqlite3_backup_init(pTo, "main", pFrom, "main");
270  if( pBackup ){
271  (void)sqlite3_backup_step(pBackup, -1);
272  (void)sqlite3_backup_finish(pBackup);
273  }
274  rc = sqlite3_errcode(pTo);
275  }
276 
277  /* Close the database connection opened on database file zFilename
278  ** and return the result of this function. */
279  (void)sqlite3_close(pFile);
280  return rc;
281 }
282 
283 bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
284 {
285  version = "0.0.0";
286  if(_ppDb)
287  {
288  UTimer timer;
289  timer.start();
290  int rc = SQLITE_OK;
291  sqlite3_stmt * ppStmt = 0;
292  std::stringstream query;
293 
294  query << "SELECT version FROM Admin;";
295 
296  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
297  if(rc == SQLITE_OK)
298  {
299  // Process the result if one
300  rc = sqlite3_step(ppStmt);
301  if(rc == SQLITE_ROW)
302  {
303  version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
304  rc = sqlite3_step(ppStmt);
305  }
306  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
307 
308  // Finalize (delete) the statement
309  rc = sqlite3_finalize(ppStmt);
310  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
311  }
312  //else
313  //{
314  // old version detected
315  //}
316  return true;
317  }
318  return false;
319 }
320 
321 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
322 {
323  this->disconnectDatabaseQuery();
324  // Open a database connection
325  _ppDb = 0;
326  _memoryUsedEstimate = 0;
327 
328  int rc = SQLITE_OK;
329  bool dbFileExist = false;
330  if(!url.empty())
331  {
332  dbFileExist = UFile::exists(url.c_str());
333  if(dbFileExist && overwritten)
334  {
335  UINFO("Deleting database %s...", url.c_str());
336  UASSERT(UFile::erase(url.c_str()) == 0);
337  dbFileExist = false;
338  }
339  else if(dbFileExist)
340  {
341  _memoryUsedEstimate = UFile::length(this->getUrl());
342  }
343  }
344 
345  if(_dbInMemory || url.empty())
346  {
347  if(!url.empty())
348  {
349  ULOGGER_INFO("Using database \"%s\" in the memory.", url.c_str());
350  }
351  else
352  {
353  ULOGGER_INFO("Using empty database in the memory.");
354  }
355  rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
356  }
357  else
358  {
359  ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
360  rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
361  }
362  if(rc != SQLITE_OK)
363  {
364  UFATAL("DB error : %s (path=\"%s\"). Make sure that your user has write "
365  "permission on the target directory (you may have to change the working directory). ", sqlite3_errmsg(_ppDb), url.c_str());
366  _ppDb = 0;
367  return false;
368  }
369 
370  if(_dbInMemory && dbFileExist)
371  {
372  UTimer timer;
373  timer.start();
374  ULOGGER_DEBUG("Loading DB ...");
375  rc = loadOrSaveDb(_ppDb, url, 0); // Load memory from file
376  ULOGGER_INFO("Loading DB time = %fs, (%s)", timer.ticks(), url.c_str());
377  if(rc != SQLITE_OK)
378  {
379  UFATAL("DB error 2 : %s", sqlite3_errmsg(_ppDb));
380  sqlite3_close(_ppDb);
381  _ppDb = 0;
382  return false;
383  }
384  }
385 
386  if(!dbFileExist)
387  {
388  if(!url.empty())
389  {
390  ULOGGER_INFO("Database \"%s\" doesn't exist, creating a new one...", url.c_str());
391  }
392  // Create the database
393  std::string schema = DATABASESCHEMA_SQL;
394  std::string targetVersion = this->getTargetVersion();
395  if(!targetVersion.empty())
396  {
397  // search for schema with version <= target version
398  std::vector<std::pair<std::string, std::string> > schemas;
399  schemas.push_back(std::make_pair("0.16.0", DATABASESCHEMA_0_16_0_SQL));
400  schemas.push_back(std::make_pair("0.16.1", DATABASESCHEMA_0_16_1_SQL));
401  schemas.push_back(std::make_pair("0.16.2", DATABASESCHEMA_0_16_2_SQL));
402  schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
403  schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
404  schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
405  schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
406  for(size_t i=0; i<schemas.size(); ++i)
407  {
408  if(uStrNumCmp(targetVersion, schemas[i].first) < 0)
409  {
410  if(i==0)
411  {
412  UERROR("Cannot create database with target version \"%s\" (not implemented), using latest version.", targetVersion.c_str());
413  }
414  break;
415  }
416  else
417  {
418  schema = schemas[i].second;
419  }
420  }
421  }
422  schema = uHex2Str(schema);
423  this->executeNoResultQuery(schema.c_str());
424  }
425  UASSERT(this->getDatabaseVersionQuery(_version)); // must be true!
426  UINFO("Database version = %s", _version.c_str());
427 
428  // From 0.11.13, compare only with minor version (patch will be used for non-database structural changes)
429  if((uStrNumCmp(_version, "0.11.12") <= 0 && uStrNumCmp(_version, RTABMAP_VERSION) > 0) ||
430  (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))
431  {
432  UERROR("Opened database version (%s) is more recent than rtabmap "
433  "installed version (%s). Please update rtabmap to new version!",
434  _version.c_str(), RTABMAP_VERSION);
435  this->disconnectDatabaseQuery(false);
436  return false;
437  }
438 
439  //Set database optimizations
440  this->setCacheSize(_cacheSize); // this will call the SQL
441  this->setJournalMode(_journalMode); // this will call the SQL
442  this->setSynchronous(_synchronous); // this will call the SQL
443  this->setTempStore(_tempStore); // this will call the SQL
444 
445  return true;
446 }
447 void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
448 {
449  UDEBUG("");
450  if(_ppDb)
451  {
452  int rc = SQLITE_OK;
453  // make sure that all statements are finalized
454  sqlite3_stmt * pStmt;
455  while( (pStmt = sqlite3_next_stmt(_ppDb, 0))!=0 )
456  {
457  rc = sqlite3_finalize(pStmt);
458  if(rc != SQLITE_OK)
459  {
460  UERROR("");
461  }
462  }
463 
464  if(save && (_dbInMemory || this->getUrl().empty()))
465  {
466  UTimer timer;
467  timer.start();
468  std::string outputFile = this->getUrl();
469  if(!outputUrl.empty())
470  {
471  outputFile = outputUrl;
472  }
473  if(outputFile.empty())
474  {
475  UWARN("Database was initialized with an empty url (in memory). To save it, "
476  "the output url should not be empty. The database is thus closed without being saved!");
477  }
478  else
479  {
480  UINFO("Saving database to %s ...", outputFile.c_str());
481  rc = loadOrSaveDb(_ppDb, outputFile, 1); // Save memory to file
482  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s), could not save \"%s\": %s. Make sure that your user has write "
483  "permission on the target directory (you may have to change the working directory). ", _version.c_str(), outputFile.c_str(), sqlite3_errmsg(_ppDb)).c_str());
484  ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
485  }
486  }
487 
488  // Then close (delete) the database connection
489  UINFO("Disconnecting database %s...", this->getUrl().c_str());
490  sqlite3_close(_ppDb);
491  _ppDb = 0;
492 
493  if(save && !_dbInMemory && !outputUrl.empty() && !this->getUrl().empty() && outputUrl.compare(this->getUrl()) != 0)
494  {
495  UWARN("Output database path (%s) is different than the opened database "
496  "path (%s). Opened database path is overwritten then renamed to output path.",
497  outputUrl.c_str(), this->getUrl().c_str());
498  if(UFile::rename(this->getUrl(), outputUrl) != 0)
499  {
500  UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
501  }
502  }
503  UINFO("Disconnected database %s!", this->getUrl().c_str());
504  }
505 }
506 
507 bool DBDriverSqlite3::isConnectedQuery() const
508 {
509  return _ppDb != 0;
510 }
511 
512 // In bytes
513 void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
514 {
515  if(_ppDb)
516  {
517  UTimer timer;
518  timer.start();
519  int rc;
520  rc = sqlite3_exec(_ppDb, sql.c_str(), 0, 0, 0);
521  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s, the query is %s", sqlite3_errmsg(_ppDb), sql.c_str()).c_str());
522  UDEBUG("Time=%fs", timer.ticks());
523  }
524 }
525 
526 unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
527 {
528  if(_dbInMemory)
529  {
530  return sqlite3_memory_used();
531  }
532  else
533  {
534  return _memoryUsedEstimate;
535  }
536 }
537 
538 long DBDriverSqlite3::getNodesMemoryUsedQuery() const
539 {
540  UDEBUG("");
541  long size = 0L;
542  if(_ppDb)
543  {
544  std::string query;
545  if(uStrNumCmp(_version, "0.18.0") >= 0)
546  {
547  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) + ifnull(length(env_sensors),0) + length(time_enter)) from Node;";
548  }
549  else if(uStrNumCmp(_version, "0.14.0") >= 0)
550  {
551  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;";
552  }
553  else if(uStrNumCmp(_version, "0.13.0") >= 0)
554  {
555  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;";
556  }
557  else if(uStrNumCmp(_version, "0.11.1") >= 0)
558  {
559  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;";
560  }
561  else if(uStrNumCmp(_version, "0.8.5") >= 0)
562  {
563  query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(time_enter)) from Node;";
564  }
565  else
566  {
567  query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose)+ length(time_enter)) from Node;";
568  }
569 
570  int rc = SQLITE_OK;
571  sqlite3_stmt * ppStmt = 0;
572  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
573  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
574  rc = sqlite3_step(ppStmt);
575  if(rc == SQLITE_ROW)
576  {
577  size = sqlite3_column_int64(ppStmt, 0);
578  rc = sqlite3_step(ppStmt);
579  }
580  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
581  rc = sqlite3_finalize(ppStmt);
582  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
583  }
584  return size;
585 }
586 long DBDriverSqlite3::getLinksMemoryUsedQuery() const
587 {
588  UDEBUG("");
589  long size = 0L;
590  if(_ppDb)
591  {
592  std::string query;
593  if(uStrNumCmp(_version, "0.13.0") >= 0)
594  {
595  query = "SELECT sum(length(type) + length(information_matrix) + length(transform) + ifnull(length(user_data),0) + length(from_id) + length(to_id)) from Link;";
596  }
597  else if(uStrNumCmp(_version, "0.10.10") >= 0)
598  {
599  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;";
600  }
601  else if(uStrNumCmp(_version, "0.8.4") >= 0)
602  {
603  query = "SELECT sum(length(type) + length(rot_variance) + length(trans_variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
604  }
605  else if(uStrNumCmp(_version, "0.7.4") >= 0)
606  {
607  query = "SELECT sum(length(type) + length(variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
608  }
609  else
610  {
611  query = "SELECT sum(length(type) + length(transform) + length(from_id) + length(to_id)) from Link;";
612  }
613 
614  int rc = SQLITE_OK;
615  sqlite3_stmt * ppStmt = 0;
616  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
617  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
618  rc = sqlite3_step(ppStmt);
619  if(rc == SQLITE_ROW)
620  {
621  size = sqlite3_column_int64(ppStmt, 0);
622  rc = sqlite3_step(ppStmt);
623  }
624  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
625  rc = sqlite3_finalize(ppStmt);
626  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
627  }
628  return size;
629 }
630 long DBDriverSqlite3::getImagesMemoryUsedQuery() const
631 {
632  UDEBUG("");
633  long size = 0L;
634  if(_ppDb)
635  {
636  std::string query;
637  if(uStrNumCmp(_version, "0.10.0") >= 0)
638  {
639  query = "SELECT sum(ifnull(length(image),0) + ifnull(length(time_enter),0)) from Data;";
640  }
641  else
642  {
643  query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Image;";
644  }
645 
646  int rc = SQLITE_OK;
647  sqlite3_stmt * ppStmt = 0;
648  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
649  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
650  rc = sqlite3_step(ppStmt);
651  if(rc == SQLITE_ROW)
652  {
653  size = sqlite3_column_int64(ppStmt, 0);
654  rc = sqlite3_step(ppStmt);
655  }
656  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
657  rc = sqlite3_finalize(ppStmt);
658  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
659  }
660  return size;
661 }
662 long DBDriverSqlite3::getDepthImagesMemoryUsedQuery() const
663 {
664  UDEBUG("");
665  long size = 0L;
666  if(_ppDb)
667  {
668  std::string query;
669  if(uStrNumCmp(_version, "0.10.0") >= 0)
670  {
671  query = "SELECT sum(ifnull(length(depth),0) + ifnull(length(time_enter),0)) from Data;";
672  }
673  else
674  {
675  query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Depth;";
676  }
677 
678  int rc = SQLITE_OK;
679  sqlite3_stmt * ppStmt = 0;
680  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
681  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
682  rc = sqlite3_step(ppStmt);
683  if(rc == SQLITE_ROW)
684  {
685  size = sqlite3_column_int64(ppStmt, 0);
686  rc = sqlite3_step(ppStmt);
687  }
688  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
689  rc = sqlite3_finalize(ppStmt);
690  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
691  }
692  return size;
693 }
694 long DBDriverSqlite3::getCalibrationsMemoryUsedQuery() const
695 {
696  UDEBUG("");
697  long size = 0L;
698  if(_ppDb)
699  {
700  std::string query;
701 
702  if(uStrNumCmp(_version, "0.10.0") >= 0)
703  {
704  query = "SELECT sum(length(calibration)) from Data;";
705  }
706  else if(uStrNumCmp(_version, "0.7.0") >= 0)
707  {
708  query = "SELECT sum(length(fx) + length(fy) + length(cx) + length(cy) + length(local_transform)) from Depth;";
709  }
710  else
711  {
712  query = "SELECT sum(length(constant) + length(local_transform)) from Depth;";
713  }
714 
715  int rc = SQLITE_OK;
716  sqlite3_stmt * ppStmt = 0;
717  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
718  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
719  rc = sqlite3_step(ppStmt);
720  if(rc == SQLITE_ROW)
721  {
722  size = sqlite3_column_int64(ppStmt, 0);
723  rc = sqlite3_step(ppStmt);
724  }
725  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
726  rc = sqlite3_finalize(ppStmt);
727  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
728  }
729  return size;
730 }
731 long DBDriverSqlite3::getGridsMemoryUsedQuery() const
732 {
733  UDEBUG("");
734  long size = 0L;
735  if(_ppDb)
736  {
737  std::string query;
738 
739  if(uStrNumCmp(_version, "0.16.0") >= 0)
740  {
741  query = "SELECT sum(ifnull(length(ground_cells),0) + ifnull(length(obstacle_cells),0) + ifnull(length(empty_cells),0) + length(cell_size) + length(view_point_x) + length(view_point_y) + length(view_point_z)) from Data;";
742  }
743  else if(uStrNumCmp(_version, "0.11.10") >= 0)
744  {
745  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;";
746  }
747  else
748  {
749  return size;
750  }
751 
752  int rc = SQLITE_OK;
753  sqlite3_stmt * ppStmt = 0;
754  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
755  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
756  rc = sqlite3_step(ppStmt);
757  if(rc == SQLITE_ROW)
758  {
759  size = sqlite3_column_int64(ppStmt, 0);
760  rc = sqlite3_step(ppStmt);
761  }
762  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
763  rc = sqlite3_finalize(ppStmt);
764  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
765  }
766  return size;
767 }
768 long DBDriverSqlite3::getLaserScansMemoryUsedQuery() const
769 {
770  UDEBUG("");
771  long size = 0L;
772  if(_ppDb)
773  {
774  std::string query;
775 
776  if(uStrNumCmp(_version, "0.11.10") >= 0)
777  {
778  query = "SELECT sum(ifnull(length(scan_info),0) + ifnull(length(scan),0)) from Data;";
779  }
780  else if(uStrNumCmp(_version, "0.10.7") >= 0)
781  {
782  query = "SELECT sum(length(scan_max_pts) + length(scan_max_range) + ifnull(length(scan),0)) from Data;";
783  }
784  else if(uStrNumCmp(_version, "0.10.0") >= 0)
785  {
786  query = "SELECT sum(length(scan_max_pts) + ifnull(length(scan),0)) from Data;";
787  }
788  else if(uStrNumCmp(_version, "0.8.11") >= 0)
789  {
790  query = "SELECT sum(length(data2d) + length(data2d_max_pts)) from Depth;";
791  }
792  else
793  {
794  query = "SELECT sum(length(data2d)) from Depth;";
795  }
796 
797  int rc = SQLITE_OK;
798  sqlite3_stmt * ppStmt = 0;
799  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
800  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
801  rc = sqlite3_step(ppStmt);
802  if(rc == SQLITE_ROW)
803  {
804  size = sqlite3_column_int64(ppStmt, 0);
805  rc = sqlite3_step(ppStmt);
806  }
807  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
808  rc = sqlite3_finalize(ppStmt);
809  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
810  }
811  return size;
812 }
813 long DBDriverSqlite3::getUserDataMemoryUsedQuery() const
814 {
815  UDEBUG("");
816  long size = 0L;
817  if(_ppDb)
818  {
819  std::string query;
820  if(uStrNumCmp(_version, "0.10.1") >= 0)
821  {
822  query = "SELECT sum(length(user_data)) from Data;";
823  }
824  else if(uStrNumCmp(_version, "0.8.8") >= 0)
825  {
826  query = "SELECT sum(length(user_data)) from Node;";
827  }
828  else
829  {
830  return size; // no user_data
831  }
832 
833  int rc = SQLITE_OK;
834  sqlite3_stmt * ppStmt = 0;
835  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
836  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
837  rc = sqlite3_step(ppStmt);
838  if(rc == SQLITE_ROW)
839  {
840  size = sqlite3_column_int64(ppStmt, 0);
841  rc = sqlite3_step(ppStmt);
842  }
843  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
844  rc = sqlite3_finalize(ppStmt);
845  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
846  }
847  return size;
848 }
849 long DBDriverSqlite3::getWordsMemoryUsedQuery() const
850 {
851  UDEBUG("");
852  long size = 0L;
853  if(_ppDb)
854  {
855  std::string query = "SELECT sum(length(id) + length(descriptor_size) + length(descriptor) + length(time_enter)) from Word;";
856 
857  int rc = SQLITE_OK;
858  sqlite3_stmt * ppStmt = 0;
859  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
860  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
861  rc = sqlite3_step(ppStmt);
862  if(rc == SQLITE_ROW)
863  {
864  size = sqlite3_column_int64(ppStmt, 0);
865  rc = sqlite3_step(ppStmt);
866  }
867  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
868  rc = sqlite3_finalize(ppStmt);
869  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
870  }
871  return size;
872 }
873 long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
874 {
875  UDEBUG("");
876  long size = 0L;
877  if(_ppDb)
878  {
879  std::string query;
880  if(uStrNumCmp(_version, "0.13.0") >= 0)
881  {
882  query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
883  "FROM Feature";
884  }
885  else if(uStrNumCmp(_version, "0.12.0") >= 0)
886  {
887  query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
888  "FROM Map_Node_Word";
889  }
890  else if(uStrNumCmp(_version, "0.11.2") >= 0)
891  {
892  query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
893  "FROM Map_Node_Word";
894  }
895  else
896  {
897  query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) "
898  "FROM Map_Node_Word";
899  }
900 
901  int rc = SQLITE_OK;
902  sqlite3_stmt * ppStmt = 0;
903  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
904  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
905  rc = sqlite3_step(ppStmt);
906  if(rc == SQLITE_ROW)
907  {
908  size = sqlite3_column_int64(ppStmt, 0);
909  rc = sqlite3_step(ppStmt);
910  }
911  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
912  rc = sqlite3_finalize(ppStmt);
913  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
914  }
915  return size;
916 }
917 long DBDriverSqlite3::getStatisticsMemoryUsedQuery() const
918 {
919  UDEBUG("");
920  long size = 0L;
921  if(_ppDb)
922  {
923  std::string query;
924  if(uStrNumCmp(_version, "0.16.2") >= 0)
925  {
926  query = "SELECT sum(length(id) + length(stamp) + ifnull(length(data),0) + ifnull(length(wm_state),0)) FROM Statistics;";
927  }
928  else if(uStrNumCmp(_version, "0.11.11") >= 0)
929  {
930  query = "SELECT sum(length(id) + length(stamp) + length(data)) FROM Statistics;";
931  }
932  else
933  {
934  return size;
935  }
936 
937  int rc = SQLITE_OK;
938  sqlite3_stmt * ppStmt = 0;
939  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
940  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
941  rc = sqlite3_step(ppStmt);
942  if(rc == SQLITE_ROW)
943  {
944  size = sqlite3_column_int64(ppStmt, 0);
945  rc = sqlite3_step(ppStmt);
946  }
947  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
948  rc = sqlite3_finalize(ppStmt);
949  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
950  }
951  return size;
952 }
953 int DBDriverSqlite3::getLastNodesSizeQuery() const
954 {
955  UDEBUG("");
956  int size = 0;
957  if(_ppDb)
958  {
959  std::string query;
960  if(uStrNumCmp(_version, "0.11.11") >= 0)
961  {
962  query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
963  }
964  else
965  {
966  query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
967  }
968 
969  int rc = SQLITE_OK;
970  sqlite3_stmt * ppStmt = 0;
971  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
972  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
973  rc = sqlite3_step(ppStmt);
974  if(rc == SQLITE_ROW)
975  {
976  size = sqlite3_column_int(ppStmt, 0);
977  rc = sqlite3_step(ppStmt);
978  }
979  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
980  rc = sqlite3_finalize(ppStmt);
981  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
982  }
983  return size;
984 }
985 int DBDriverSqlite3::getLastDictionarySizeQuery() const
986 {
987  UDEBUG("");
988  int size = 0;
989  if(_ppDb)
990  {
991  std::string query;
992  if(uStrNumCmp(_version, "0.11.11") >= 0)
993  {
994  query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
995  }
996  else
997  {
998  query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
999  }
1000 
1001  int rc = SQLITE_OK;
1002  sqlite3_stmt * ppStmt = 0;
1003  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
1004  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1005  rc = sqlite3_step(ppStmt);
1006  if(rc == SQLITE_ROW)
1007  {
1008  size = sqlite3_column_int(ppStmt, 0);
1009  rc = sqlite3_step(ppStmt);
1010  }
1011  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1012  rc = sqlite3_finalize(ppStmt);
1013  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1014  }
1015  return size;
1016 }
1017 int DBDriverSqlite3::getTotalNodesSizeQuery() const
1018 {
1019  UDEBUG("");
1020  int size = 0;
1021  if(_ppDb)
1022  {
1023  std::string query = "SELECT count(id) from Node;";
1024 
1025  int rc = SQLITE_OK;
1026  sqlite3_stmt * ppStmt = 0;
1027  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
1028  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1029  rc = sqlite3_step(ppStmt);
1030  if(rc == SQLITE_ROW)
1031  {
1032  size = sqlite3_column_int(ppStmt, 0);
1033  rc = sqlite3_step(ppStmt);
1034  }
1035  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1036  rc = sqlite3_finalize(ppStmt);
1037  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1038  }
1039  return size;
1040 }
1041 int DBDriverSqlite3::getTotalDictionarySizeQuery() const
1042 {
1043  UDEBUG("");
1044  int size = 0;
1045  if(_ppDb)
1046  {
1047  std::string query = "SELECT count(id) from Word;";
1048 
1049  int rc = SQLITE_OK;
1050  sqlite3_stmt * ppStmt = 0;
1051  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
1052  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1053  rc = sqlite3_step(ppStmt);
1054  if(rc == SQLITE_ROW)
1055  {
1056  size = sqlite3_column_int(ppStmt, 0);
1057  rc = sqlite3_step(ppStmt);
1058  }
1059  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1060  rc = sqlite3_finalize(ppStmt);
1061  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1062  }
1063  return size;
1064 }
1065 
1066 ParametersMap DBDriverSqlite3::getLastParametersQuery() const
1067 {
1068  UDEBUG("");
1069  ParametersMap parameters;
1070  if(_ppDb)
1071  {
1072  if(uStrNumCmp(_version, "0.11.8") >= 0)
1073  {
1074  std::string query;
1075  if(uStrNumCmp(_version, "0.11.11") >= 0)
1076  {
1077  query = "SELECT parameters "
1078  "FROM Info "
1079  "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
1080  }
1081  else
1082  {
1083  query = "SELECT parameters "
1084  "FROM Statistics "
1085  "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
1086  }
1087 
1088  int rc = SQLITE_OK;
1089  sqlite3_stmt * ppStmt = 0;
1090  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
1091  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1092  rc = sqlite3_step(ppStmt);
1093  if(rc == SQLITE_ROW)
1094  {
1095  std::string text((const char *)sqlite3_column_text(ppStmt, 0));
1096 
1097  if(text.size())
1098  {
1099  parameters = Parameters::deserialize(text);
1100  }
1101 
1102  rc = sqlite3_step(ppStmt);
1103  }
1104  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1105  rc = sqlite3_finalize(ppStmt);
1106  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1107  }
1108  }
1109  return parameters;
1110 }
1111 
1112 std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const
1113 {
1114  UDEBUG("nodeId=%d", nodeId);
1115  std::map<std::string, float> data;
1116  if(_ppDb)
1117  {
1118  if(uStrNumCmp(_version, "0.11.11") >= 0)
1119  {
1120  std::stringstream query;
1121 
1122  if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
1123  {
1124  query << "SELECT stamp, data, wm_state "
1125  << "FROM Statistics "
1126  << "WHERE id=" << nodeId << ";";
1127  }
1128  else
1129  {
1130  query << "SELECT stamp, data "
1131  << "FROM Statistics "
1132  << "WHERE id=" << nodeId << ";";
1133  }
1134 
1135  int rc = SQLITE_OK;
1136  sqlite3_stmt * ppStmt = 0;
1137  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
1138  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1139  rc = sqlite3_step(ppStmt);
1140  if(rc == SQLITE_ROW)
1141  {
1142  int index = 0;
1143  stamp = sqlite3_column_double(ppStmt, index++);
1144 
1145  std::string text;
1146  if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
1147  {
1148  const void * dataPtr = sqlite3_column_blob(ppStmt, index);
1149  int dataSize = sqlite3_column_bytes(ppStmt, index++);
1150  if(dataSize>0 && dataPtr)
1151  {
1152  text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
1153  }
1154  }
1155  else
1156  {
1157  text = (const char *)sqlite3_column_text(ppStmt, index++);
1158  }
1159 
1160  if(text.size())
1161  {
1162  data = Statistics::deserializeData(text);
1163  }
1164 
1165  if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
1166  {
1167  const void * dataPtr = sqlite3_column_blob(ppStmt, index);
1168  int dataSize = sqlite3_column_bytes(ppStmt, index++);
1169  if(dataSize>0 && dataPtr)
1170  {
1171  cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
1172  UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
1173  wmState->resize(wmStateMat.cols);
1174  memcpy(wmState->data(), wmStateMat.data, wmState->size()*sizeof(int));
1175  }
1176  }
1177 
1178  rc = sqlite3_step(ppStmt);
1179  }
1180  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1181  rc = sqlite3_finalize(ppStmt);
1182  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1183  }
1184  }
1185  return data;
1186 }
1187 
1188 std::map<int, std::pair<std::map<std::string, float>, double> > DBDriverSqlite3::getAllStatisticsQuery() const
1189 {
1190  UDEBUG("");
1191  std::map<int, std::pair<std::map<std::string, float>, double> > data;
1192  if(_ppDb)
1193  {
1194  if(uStrNumCmp(_version, "0.11.11") >= 0)
1195  {
1196  std::stringstream query;
1197 
1198  query << "SELECT id, stamp, data "
1199  << "FROM Statistics;";
1200 
1201  int rc = SQLITE_OK;
1202  sqlite3_stmt * ppStmt = 0;
1203  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
1204  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1205  rc = sqlite3_step(ppStmt);
1206  while(rc == SQLITE_ROW)
1207  {
1208  int index = 0;
1209  int id = sqlite3_column_int(ppStmt, index++);
1210  double stamp = sqlite3_column_double(ppStmt, index++);
1211 
1212  std::string text;
1213  if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
1214  {
1215  const void * dataPtr = 0;
1216  int dataSize = 0;
1217  dataPtr = sqlite3_column_blob(ppStmt, index);
1218  dataSize = sqlite3_column_bytes(ppStmt, index++);
1219  if(dataSize>0 && dataPtr)
1220  {
1221  text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
1222  }
1223  }
1224  else
1225  {
1226  text = (const char *)sqlite3_column_text(ppStmt, index++);
1227  }
1228 
1229  if(text.size())
1230  {
1231  data.insert(std::make_pair(id, std::make_pair(Statistics::deserializeData(text), stamp)));
1232  }
1233 
1234  rc = sqlite3_step(ppStmt);
1235  }
1236  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1237  rc = sqlite3_finalize(ppStmt);
1238  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1239  }
1240  }
1241  UDEBUG("");
1242  return data;
1243 }
1244 
1245 std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery() const
1246 {
1247  UDEBUG("");
1248  std::map<int, std::vector<int> > data;
1249  if(_ppDb)
1250  {
1251  if(uStrNumCmp(_version, "0.16.2") >= 0)
1252  {
1253  std::stringstream query;
1254 
1255  query << "SELECT id, wm_state "
1256  << "FROM Statistics;";
1257 
1258  int rc = SQLITE_OK;
1259  sqlite3_stmt * ppStmt = 0;
1260  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
1261  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1262  rc = sqlite3_step(ppStmt);
1263  while(rc == SQLITE_ROW)
1264  {
1265  int index = 0;
1266  int id = sqlite3_column_int(ppStmt, index++);
1267 
1268  std::vector<int> wmState;
1269  const void * dataPtr = sqlite3_column_blob(ppStmt, index);
1270  int dataSize = sqlite3_column_bytes(ppStmt, index++);
1271  if(dataSize>0 && dataPtr)
1272  {
1273  cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
1274  UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
1275  wmState.resize(wmStateMat.cols);
1276  memcpy(wmState.data(), wmStateMat.data, wmState.size()*sizeof(int));
1277  }
1278 
1279  if(!wmState.empty())
1280  {
1281  data.insert(std::make_pair(id, wmState));
1282  }
1283 
1284  rc = sqlite3_step(ppStmt);
1285  }
1286  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1287  rc = sqlite3_finalize(ppStmt);
1288  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1289  }
1290  }
1291  UDEBUG("");
1292  return data;
1293 }
1294 
1295 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
1296 {
1297  UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
1298  (int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
1299 
1300  if(!images && !scan && !userData && !occupancyGrid)
1301  {
1302  UWARN("All requested data fields are false! Nothing loaded...");
1303  return;
1304  }
1305 
1306  if(_ppDb)
1307  {
1308  UTimer timer;
1309  timer.start();
1310  int rc = SQLITE_OK;
1311  sqlite3_stmt * ppStmt = 0;
1312  std::stringstream query;
1313 
1314  if(uStrNumCmp(_version, "0.11.10") >= 0)
1315  {
1316  std::stringstream fields;
1317 
1318  if(images)
1319  {
1320  fields << "image, depth, calibration";
1321  if(scan || userData || occupancyGrid)
1322  {
1323  fields << ", ";
1324  }
1325  }
1326  if(scan)
1327  {
1328  fields << "scan_info, scan";
1329  if(userData || occupancyGrid)
1330  {
1331  fields << ", ";
1332  }
1333  }
1334  if(userData)
1335  {
1336  fields << "user_data";
1337  if(occupancyGrid)
1338  {
1339  fields << ", ";
1340  }
1341  }
1342  if(occupancyGrid)
1343  {
1344  if(uStrNumCmp(_version, "0.16.0") >= 0)
1345  {
1346  fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
1347  }
1348  else
1349  {
1350  fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
1351  }
1352  }
1353 
1354  query << "SELECT " << fields.str().c_str() << " "
1355  << "FROM Data "
1356  << "WHERE id = ?"
1357  <<";";
1358  }
1359  else if(uStrNumCmp(_version, "0.10.7") >= 0)
1360  {
1361  query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
1362  << "FROM Data "
1363  << "WHERE id = ?"
1364  <<";";
1365  }
1366  else if(uStrNumCmp(_version, "0.10.1") >= 0)
1367  {
1368  query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
1369  << "FROM Data "
1370  << "WHERE id = ?"
1371  <<";";
1372  }
1373  else if(uStrNumCmp(_version, "0.10.0") >= 0)
1374  {
1375  query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data "
1376  << "FROM Data "
1377  << "INNER JOIN Node "
1378  << "ON Data.id = Node.id "
1379  << "WHERE Data.id = ?"
1380  <<";";
1381  }
1382  else if(uStrNumCmp(_version, "0.8.11") >= 0)
1383  {
1384  query << "SELECT Image.data, "
1385  "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data "
1386  << "FROM Image "
1387  << "INNER JOIN Node "
1388  << "on Image.id = Node.id "
1389  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
1390  << "ON Image.id = Depth.id "
1391  << "WHERE Image.id = ?"
1392  <<";";
1393  }
1394  else if(uStrNumCmp(_version, "0.8.8") >= 0)
1395  {
1396  query << "SELECT Image.data, "
1397  "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data "
1398  << "FROM Image "
1399  << "INNER JOIN Node "
1400  << "on Image.id = Node.id "
1401  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
1402  << "ON Image.id = Depth.id "
1403  << "WHERE Image.id = ?"
1404  <<";";
1405  }
1406  else if(uStrNumCmp(_version, "0.7.0") >= 0)
1407  {
1408  query << "SELECT Image.data, "
1409  "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
1410  << "FROM Image "
1411  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
1412  << "ON Image.id = Depth.id "
1413  << "WHERE Image.id = ?"
1414  <<";";
1415  }
1416  else
1417  {
1418  query << "SELECT Image.data, "
1419  "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
1420  << "FROM Image "
1421  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
1422  << "ON Image.id = Depth.id "
1423  << "WHERE Image.id = ?"
1424  <<";";
1425  }
1426 
1427  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
1428  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1429 
1430  const void * data = 0;
1431  int dataSize = 0;
1432  int index = 0;
1433 
1434  for(std::list<Signature*>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
1435  {
1436  UASSERT(*iter != 0);
1437 
1438  ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
1439  // bind id
1440  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
1441  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1442 
1443  // Process the result if one
1444  rc = sqlite3_step(ppStmt);
1445  if(rc == SQLITE_ROW)
1446  {
1447  index = 0;
1448 
1449  cv::Mat imageCompressed;
1450  cv::Mat depthOrRightCompressed;
1451  std::vector<CameraModel> models;
1452  std::vector<StereoCameraModel> stereoModels;
1453  Transform localTransform = Transform::getIdentity();
1454  cv::Mat scanCompressed;
1455  cv::Mat userDataCompressed;
1456 
1457  if(uStrNumCmp(_version, "0.11.10") < 0 || images)
1458  {
1459  //Create the image
1460  data = sqlite3_column_blob(ppStmt, index);
1461  dataSize = sqlite3_column_bytes(ppStmt, index++);
1462  if(dataSize>4 && data)
1463  {
1464  imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
1465  }
1466 
1467  //Create the depth image
1468  data = sqlite3_column_blob(ppStmt, index);
1469  dataSize = sqlite3_column_bytes(ppStmt, index++);
1470  if(dataSize>4 && data)
1471  {
1472  depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
1473  }
1474 
1475  if(uStrNumCmp(_version, "0.10.0") < 0)
1476  {
1477  data = sqlite3_column_blob(ppStmt, index); // local transform
1478  dataSize = sqlite3_column_bytes(ppStmt, index++);
1479  if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
1480  {
1481  memcpy(localTransform.data(), data, dataSize);
1482  if(uStrNumCmp(_version, "0.15.2") < 0)
1483  {
1484  localTransform.normalizeRotation();
1485  }
1486  }
1487  }
1488 
1489  // calibration
1490  if(uStrNumCmp(_version, "0.10.0") >= 0)
1491  {
1492  data = sqlite3_column_blob(ppStmt, index);
1493  dataSize = sqlite3_column_bytes(ppStmt, index++);
1494  // multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
1495  // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
1496  if(dataSize > 0 && data)
1497  {
1498  if(uStrNumCmp(_version, "0.18.0") >= 0)
1499  {
1500  if(dataSize >= int(sizeof(int)*4))
1501  {
1502  const int * dataInt = (const int *)data;
1503  int type = dataInt[3];
1504  if(type == 0) // mono
1505  {
1506  CameraModel model;
1507  int bytesReadTotal = 0;
1508  unsigned int bytesRead = 0;
1509  while(bytesReadTotal < dataSize &&
1510  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
1511  {
1512  bytesReadTotal+=bytesRead;
1513  models.push_back(model);
1514  }
1515  UASSERT(bytesReadTotal == dataSize);
1516  }
1517  else if(type == 1) // stereo
1518  {
1519  StereoCameraModel model;
1520  int bytesReadTotal = 0;
1521  unsigned int bytesRead = 0;
1522  while(bytesReadTotal < dataSize &&
1523  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
1524  {
1525  bytesReadTotal+=bytesRead;
1526  stereoModels.push_back(model);
1527  }
1528  UASSERT(bytesReadTotal == dataSize);
1529  }
1530  else
1531  {
1532  UFATAL("Unknown calibration type %d", type);
1533  }
1534  }
1535  else
1536  {
1537  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
1538  }
1539  }
1540  else
1541  {
1542  float * dataFloat = (float*)data;
1543  if(uStrNumCmp(_version, "0.11.2") >= 0 &&
1544  (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
1545  {
1546  int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
1547  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
1548  int max = cameraCount*(6+localTransform.size());
1549  for(int i=0; i<max; i+=6+localTransform.size())
1550  {
1551  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
1552  localTransform = Transform::getIdentity();
1553  memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
1554  if(uStrNumCmp(_version, "0.15.2") < 0)
1555  {
1556  localTransform.normalizeRotation();
1557  }
1558  models.push_back(CameraModel(
1559  (double)dataFloat[i],
1560  (double)dataFloat[i+1],
1561  (double)dataFloat[i+2],
1562  (double)dataFloat[i+3],
1563  localTransform));
1564  models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
1565  UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
1566  dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
1567  localTransform.prettyPrint().c_str());
1568  }
1569  }
1570  else if(uStrNumCmp(_version, "0.11.2") < 0 &&
1571  (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
1572  {
1573  int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
1574  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
1575  int max = cameraCount*(4+localTransform.size());
1576  for(int i=0; i<max; i+=4+localTransform.size())
1577  {
1578  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
1579  localTransform = Transform::getIdentity();
1580  memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
1581  if(uStrNumCmp(_version, "0.15.2") < 0)
1582  {
1583  localTransform.normalizeRotation();
1584  }
1585  models.push_back(CameraModel(
1586  (double)dataFloat[i],
1587  (double)dataFloat[i+1],
1588  (double)dataFloat[i+2],
1589  (double)dataFloat[i+3],
1590  localTransform));
1591  }
1592  }
1593  else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
1594  {
1595  UDEBUG("Loading calibration of a stereo camera");
1596  memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
1597  if(uStrNumCmp(_version, "0.15.2") < 0)
1598  {
1599  localTransform.normalizeRotation();
1600  }
1601  stereoModels.push_back(StereoCameraModel(
1602  dataFloat[0], // fx
1603  dataFloat[1], // fy
1604  dataFloat[2], // cx
1605  dataFloat[3], // cy
1606  dataFloat[4], // baseline
1607  localTransform,
1608  cv::Size(dataFloat[5],dataFloat[6])));
1609  }
1610  else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
1611  {
1612  UDEBUG("Loading calibration of a stereo camera");
1613  memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
1614  if(uStrNumCmp(_version, "0.15.2") < 0)
1615  {
1616  localTransform.normalizeRotation();
1617  }
1618  stereoModels.push_back(StereoCameraModel(
1619  dataFloat[0], // fx
1620  dataFloat[1], // fy
1621  dataFloat[2], // cx
1622  dataFloat[3], // cy
1623  dataFloat[4], // baseline
1624  localTransform));
1625  }
1626  else
1627  {
1628  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
1629  }
1630  }
1631  }
1632 
1633  }
1634  else if(uStrNumCmp(_version, "0.7.0") >= 0)
1635  {
1636  UDEBUG("Loading calibration version >= 0.7.0");
1637  double fx = sqlite3_column_double(ppStmt, index++);
1638  double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
1639  double cx = sqlite3_column_double(ppStmt, index++);
1640  double cy = sqlite3_column_double(ppStmt, index++);
1641  if(fyOrBaseline < 1.0)
1642  {
1643  //it is a baseline
1644  stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
1645  }
1646  else
1647  {
1648  models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
1649  }
1650  }
1651  else
1652  {
1653  UDEBUG("Loading calibration version < 0.7.0");
1654  float depthConstant = sqlite3_column_double(ppStmt, index++);
1655  float fx = 1.0f/depthConstant;
1656  float fy = 1.0f/depthConstant;
1657  float cx = 0.0f;
1658  float cy = 0.0f;
1659  models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
1660  }
1661  }
1662 
1663  int laserScanMaxPts = 0;
1664  float laserScanMinRange = 0.0f;
1665  float laserScanMaxRange = 0.0f;
1666  int laserScanFormat = 0;
1667  float laserScanAngleMin = 0.0f;
1668  float laserScanAngleMax = 0.0f;
1669  float laserScanAngleInc = 0.0f;
1670  Transform scanLocalTransform = Transform::getIdentity();
1671  if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
1672  {
1673  // scan_info
1674  if(uStrNumCmp(_version, "0.11.10") >= 0)
1675  {
1676  data = sqlite3_column_blob(ppStmt, index);
1677  dataSize = sqlite3_column_bytes(ppStmt, index++);
1678 
1679  if(dataSize > 0 && data)
1680  {
1681  float * dataFloat = (float*)data;
1682 
1683  if(uStrNumCmp(_version, "0.18.0") >= 0)
1684  {
1685  UASSERT(dataSize == (int)((scanLocalTransform.size()+7)*sizeof(float)));
1686  laserScanFormat = (int)dataFloat[0];
1687  laserScanMinRange = dataFloat[1];
1688  laserScanMaxRange = dataFloat[2];
1689  laserScanAngleMin = dataFloat[3];
1690  laserScanAngleMax = dataFloat[4];
1691  laserScanAngleInc = dataFloat[5];
1692  laserScanMaxPts = (int)dataFloat[6];
1693  memcpy(scanLocalTransform.data(), dataFloat+7, scanLocalTransform.size()*sizeof(float));
1694  }
1695  else
1696  {
1697  if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((scanLocalTransform.size()+3)*sizeof(float)))
1698  {
1699  // new in 0.16.1
1700  laserScanFormat = (int)dataFloat[2];
1701  memcpy(scanLocalTransform.data(), dataFloat+3, scanLocalTransform.size()*sizeof(float));
1702  }
1703  else if(dataSize == (int)((scanLocalTransform.size()+2)*sizeof(float)))
1704  {
1705  memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
1706  }
1707  else
1708  {
1709  UFATAL("Unexpected size %d for laser scan info!", dataSize);
1710  }
1711 
1712  if(uStrNumCmp(_version, "0.15.2") < 0)
1713  {
1714  scanLocalTransform.normalizeRotation();
1715  }
1716  laserScanMaxPts = (int)dataFloat[0];
1717  laserScanMaxRange = dataFloat[1];
1718  }
1719  }
1720  }
1721  else
1722  {
1723  if(uStrNumCmp(_version, "0.8.11") >= 0)
1724  {
1725  laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
1726  }
1727 
1728  if(uStrNumCmp(_version, "0.10.7") >= 0)
1729  {
1730  laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
1731  }
1732  }
1733 
1734  data = sqlite3_column_blob(ppStmt, index);
1735  dataSize = sqlite3_column_bytes(ppStmt, index++);
1736  //Create the laserScan
1737  if(dataSize>4 && data)
1738  {
1739  scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
1740  }
1741  }
1742 
1743  if(uStrNumCmp(_version, "0.11.10") < 0 || userData)
1744  {
1745  if(uStrNumCmp(_version, "0.8.8") >= 0)
1746  {
1747  data = sqlite3_column_blob(ppStmt, index);
1748  dataSize = sqlite3_column_bytes(ppStmt, index++);
1749  //Create the userData
1750  if(dataSize>4 && data)
1751  {
1752  if(uStrNumCmp(_version, "0.10.1") >= 0)
1753  {
1754  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
1755  }
1756  else
1757  {
1758  // compress data (set uncompressed data to signed to make difference with compressed type)
1759  userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
1760  }
1761  }
1762  }
1763  }
1764 
1765  // Occupancy grid
1766  cv::Mat groundCellsCompressed;
1767  cv::Mat obstacleCellsCompressed;
1768  cv::Mat emptyCellsCompressed;
1769  float cellSize = 0.0f;
1770  cv::Point3f viewPoint;
1771  if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
1772  {
1773  // ground
1774  data = sqlite3_column_blob(ppStmt, index);
1775  dataSize = sqlite3_column_bytes(ppStmt, index++);
1776  if(dataSize > 0 && data)
1777  {
1778  groundCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
1779  memcpy((void*)groundCellsCompressed.data, data, dataSize);
1780  }
1781 
1782  // obstacle
1783  data = sqlite3_column_blob(ppStmt, index);
1784  dataSize = sqlite3_column_bytes(ppStmt, index++);
1785  if(dataSize > 0 && data)
1786  {
1787  obstacleCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
1788  memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
1789  }
1790 
1791  if(uStrNumCmp(_version, "0.16.0") >= 0)
1792  {
1793  // empty
1794  data = sqlite3_column_blob(ppStmt, index);
1795  dataSize = sqlite3_column_bytes(ppStmt, index++);
1796  if(dataSize > 0 && data)
1797  {
1798  emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
1799  memcpy((void*)emptyCellsCompressed.data, data, dataSize);
1800  }
1801  }
1802 
1803  cellSize = sqlite3_column_double(ppStmt, index++);
1804  viewPoint.x = sqlite3_column_double(ppStmt, index++);
1805  viewPoint.y = sqlite3_column_double(ppStmt, index++);
1806  viewPoint.z = sqlite3_column_double(ppStmt, index++);
1807  }
1808 
1809  if(scan)
1810  {
1811  LaserScan laserScan;
1812  if(laserScanAngleMin < laserScanAngleMax && laserScanAngleInc != 0.0f)
1813  {
1814  laserScan = LaserScan(scanCompressed, (LaserScan::Format)laserScanFormat, laserScanMinRange, laserScanMaxRange, laserScanAngleMin, laserScanAngleMax, laserScanAngleInc, scanLocalTransform);
1815  }
1816  else
1817  {
1818  laserScan = LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform);
1819  }
1820  (*iter)->sensorData().setLaserScan(laserScan);
1821  }
1822  if(images)
1823  {
1824  if(models.size())
1825  {
1826  (*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
1827  }
1828  else
1829  {
1830  (*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModels);
1831  }
1832  }
1833  if(userData)
1834  {
1835  (*iter)->sensorData().setUserData(userDataCompressed);
1836  }
1837 
1838  if(occupancyGrid)
1839  {
1840  (*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
1841  }
1842 
1843  rc = sqlite3_step(ppStmt); // next result...
1844  }
1845  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1846 
1847  //reset
1848  rc = sqlite3_reset(ppStmt);
1849  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1850  }
1851 
1852  // Finalize (delete) the statement
1853  rc = sqlite3_finalize(ppStmt);
1854  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1855  ULOGGER_DEBUG("Time=%fs", timer.ticks());
1856  }
1857 }
1858 
1859 bool DBDriverSqlite3::getCalibrationQuery(
1860  int signatureId,
1861  std::vector<CameraModel> & models,
1862  std::vector<StereoCameraModel> & stereoModels) const
1863 {
1864  bool found = false;
1865  if(_ppDb && signatureId)
1866  {
1867  int rc = SQLITE_OK;
1868  sqlite3_stmt * ppStmt = 0;
1869  std::stringstream query;
1870 
1871  if(uStrNumCmp(_version, "0.10.0") >= 0)
1872  {
1873  query << "SELECT calibration "
1874  << "FROM Data "
1875  << "WHERE id = " << signatureId
1876  <<";";
1877  }
1878  else if(uStrNumCmp(_version, "0.7.0") >= 0)
1879  {
1880  query << "SELECT local_transform, fx, fy, cx, cy "
1881  << "FROM Depth "
1882  << "WHERE id = " << signatureId
1883  <<";";
1884  }
1885  else
1886  {
1887  query << "SELECT local_transform, constant "
1888  << "FROM Depth "
1889  << "WHERE id = " << signatureId
1890  <<";";
1891  }
1892 
1893  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
1894  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
1895 
1896  const void * data = 0;
1897  int dataSize = 0;
1898  Transform localTransform = Transform::getIdentity();
1899 
1900  // Process the result if one
1901  rc = sqlite3_step(ppStmt);
1902  if(rc == SQLITE_ROW)
1903  {
1904  found = true;
1905  int index = 0;
1906 
1907  // calibration
1908  if(uStrNumCmp(_version, "0.10.0") < 0)
1909  {
1910  data = sqlite3_column_blob(ppStmt, index); // local transform
1911  dataSize = sqlite3_column_bytes(ppStmt, index++);
1912  if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
1913  {
1914  memcpy(localTransform.data(), data, dataSize);
1915  localTransform.normalizeRotation();
1916  }
1917  }
1918 
1919  if(uStrNumCmp(_version, "0.10.0") >= 0)
1920  {
1921  data = sqlite3_column_blob(ppStmt, index);
1922  dataSize = sqlite3_column_bytes(ppStmt, index++);
1923  // multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
1924  // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
1925  if(dataSize > 0 && data)
1926  {
1927  if(uStrNumCmp(_version, "0.18.0") >= 0)
1928  {
1929  if(dataSize >= int(sizeof(int)*4))
1930  {
1931  const int * dataInt = (const int *)data;
1932  int type = dataInt[3];
1933  if(type == 0) // mono
1934  {
1935  CameraModel model;
1936  int bytesReadTotal = 0;
1937  unsigned int bytesRead = 0;
1938  while(bytesReadTotal < dataSize &&
1939  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
1940  {
1941  bytesReadTotal+=bytesRead;
1942  models.push_back(model);
1943  }
1944  UASSERT(bytesReadTotal == dataSize);
1945  }
1946  else if(type == 1) // stereo
1947  {
1948  StereoCameraModel model;
1949  int bytesReadTotal = 0;
1950  unsigned int bytesRead = 0;
1951  while(bytesReadTotal < dataSize &&
1952  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
1953  {
1954  bytesReadTotal+=bytesRead;
1955  stereoModels.push_back(model);
1956  }
1957  UASSERT(bytesReadTotal == dataSize);
1958  }
1959  else
1960  {
1961  UFATAL("Unknown calibration type %d", type);
1962  }
1963  }
1964  else
1965  {
1966  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
1967  }
1968  }
1969  else
1970  {
1971  float * dataFloat = (float*)data;
1972  if(uStrNumCmp(_version, "0.11.2") >= 0 &&
1973  (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
1974  {
1975  int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
1976  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
1977  int max = cameraCount*(6+localTransform.size());
1978  for(int i=0; i<max; i+=6+localTransform.size())
1979  {
1980  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
1981  localTransform = Transform::getIdentity();
1982  memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
1983  if(uStrNumCmp(_version, "0.15.2") < 0)
1984  {
1985  localTransform.normalizeRotation();
1986  }
1987  models.push_back(CameraModel(
1988  (double)dataFloat[i],
1989  (double)dataFloat[i+1],
1990  (double)dataFloat[i+2],
1991  (double)dataFloat[i+3],
1992  localTransform));
1993  models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
1994  UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
1995  dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
1996  localTransform.prettyPrint().c_str());
1997  }
1998  }
1999  else if(uStrNumCmp(_version, "0.11.2") < 0 &&
2000  (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
2001  {
2002  int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
2003  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
2004  int max = cameraCount*(4+localTransform.size());
2005  for(int i=0; i<max; i+=4+localTransform.size())
2006  {
2007  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
2008  localTransform = Transform::getIdentity();
2009  memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
2010  if(uStrNumCmp(_version, "0.15.2") < 0)
2011  {
2012  localTransform.normalizeRotation();
2013  }
2014  models.push_back(CameraModel(
2015  (double)dataFloat[i],
2016  (double)dataFloat[i+1],
2017  (double)dataFloat[i+2],
2018  (double)dataFloat[i+3],
2019  localTransform));
2020  }
2021  }
2022  else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
2023  {
2024  UDEBUG("Loading calibration of a stereo camera");
2025  memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
2026  if(uStrNumCmp(_version, "0.15.2") < 0)
2027  {
2028  localTransform.normalizeRotation();
2029  }
2030  stereoModels.push_back(StereoCameraModel(
2031  dataFloat[0], // fx
2032  dataFloat[1], // fy
2033  dataFloat[2], // cx
2034  dataFloat[3], // cy
2035  dataFloat[4], // baseline
2036  localTransform,
2037  cv::Size(dataFloat[5],dataFloat[6])));
2038  }
2039  else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
2040  {
2041  UDEBUG("Loading calibration of a stereo camera");
2042  memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
2043  if(uStrNumCmp(_version, "0.15.2") < 0)
2044  {
2045  localTransform.normalizeRotation();
2046  }
2047  stereoModels.push_back((StereoCameraModel(
2048  dataFloat[0], // fx
2049  dataFloat[1], // fy
2050  dataFloat[2], // cx
2051  dataFloat[3], // cy
2052  dataFloat[4], // baseline
2053  localTransform)));
2054  }
2055  else
2056  {
2057  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
2058  }
2059  }
2060  }
2061 
2062  }
2063  else if(uStrNumCmp(_version, "0.7.0") >= 0)
2064  {
2065  UDEBUG("Loading calibration version >= 0.7.0");
2066  double fx = sqlite3_column_double(ppStmt, index++);
2067  double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
2068  double cx = sqlite3_column_double(ppStmt, index++);
2069  double cy = sqlite3_column_double(ppStmt, index++);
2070  UDEBUG("fx=%f fyOrBaseline=%f cx=%f cy=%f", fx, fyOrBaseline, cx, cy);
2071  if(fyOrBaseline < 1.0)
2072  {
2073  //it is a baseline
2074  stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
2075  }
2076  else
2077  {
2078  models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
2079  }
2080  }
2081  else
2082  {
2083  UDEBUG("Loading calibration version < 0.7.0");
2084  float depthConstant = sqlite3_column_double(ppStmt, index++);
2085  float fx = 1.0f/depthConstant;
2086  float fy = 1.0f/depthConstant;
2087  float cx = 0.0f;
2088  float cy = 0.0f;
2089  models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
2090  }
2091 
2092  rc = sqlite3_step(ppStmt); // next result...
2093  }
2094  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2095 
2096  // Finalize (delete) the statement
2097  rc = sqlite3_finalize(ppStmt);
2098  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2099  }
2100  return found;
2101 }
2102 
2103 bool DBDriverSqlite3::getLaserScanInfoQuery(
2104  int signatureId,
2105  LaserScan & info) const
2106 {
2107  bool found = false;
2108  if(_ppDb && signatureId)
2109  {
2110  int rc = SQLITE_OK;
2111  sqlite3_stmt * ppStmt = 0;
2112  std::stringstream query;
2113 
2114  if(uStrNumCmp(_version, "0.11.10") >= 0)
2115  {
2116  query << "SELECT scan_info "
2117  << "FROM Data "
2118  << "WHERE id = " << signatureId
2119  <<";";
2120  }
2121  else
2122  {
2123  return false;
2124  }
2125 
2126  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2127  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2128 
2129  const void * data = 0;
2130  int dataSize = 0;
2131  Transform localTransform = Transform::getIdentity();
2132  int format = 0;
2133  int maxPts = 0;
2134  float minRange = 0.0f;
2135  float maxRange = 0.0f;
2136  float angleMin = 0.0f;
2137  float angleMax = 0.0f;
2138  float angleInc = 0.0f;
2139 
2140  // Process the result if one
2141  rc = sqlite3_step(ppStmt);
2142  if(rc == SQLITE_ROW)
2143  {
2144  found = true;
2145  int index = 0;
2146 
2147  // scan_info
2148  data = sqlite3_column_blob(ppStmt, index);
2149  dataSize = sqlite3_column_bytes(ppStmt, index++);
2150 
2151  if(dataSize > 0 && data)
2152  {
2153  float * dataFloat = (float*)data;
2154  if(uStrNumCmp(_version, "0.18.0") >= 0)
2155  {
2156  UASSERT(dataSize == (int)((localTransform.size()+7)*sizeof(float)));
2157  format = (int)dataFloat[0];
2158  minRange = dataFloat[1];
2159  maxRange = dataFloat[2];
2160  angleMin = dataFloat[3];
2161  angleMax = dataFloat[4];
2162  angleInc = dataFloat[5];
2163  maxPts = (int)dataFloat[6];
2164  memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
2165  }
2166  else
2167  {
2168  if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((localTransform.size()+3)*sizeof(float)))
2169  {
2170  // new in 0.16.1
2171  format = (int)dataFloat[2];
2172  memcpy(localTransform.data(), dataFloat+3, localTransform.size()*sizeof(float));
2173  }
2174  else if(dataSize == (int)((localTransform.size()+2)*sizeof(float)))
2175  {
2176  memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
2177  }
2178  else
2179  {
2180  UFATAL("Unexpected size %d for laser scan info!", dataSize);
2181  }
2182  if(uStrNumCmp(_version, "0.15.2") < 0)
2183  {
2184  localTransform.normalizeRotation();
2185  }
2186  maxPts = (int)dataFloat[0];
2187  maxRange = dataFloat[1];
2188  }
2189 
2190  if(angleInc != 0.0f && angleMin < angleMax)
2191  {
2192  info = LaserScan(cv::Mat(), (LaserScan::Format)format, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
2193  }
2194  else
2195  {
2196  info = LaserScan(cv::Mat(), maxPts, maxRange, (LaserScan::Format)format, localTransform);
2197  }
2198  }
2199 
2200  rc = sqlite3_step(ppStmt); // next result...
2201  }
2202  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2203 
2204  // Finalize (delete) the statement
2205  rc = sqlite3_finalize(ppStmt);
2206  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2207  }
2208  return found;
2209 }
2210 
2211 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
2212  Transform & pose,
2213  int & mapId,
2214  int & weight,
2215  std::string & label,
2216  double & stamp,
2217  Transform & groundTruthPose,
2218  std::vector<float> & velocity,
2219  GPS & gps,
2220  EnvSensors & sensors) const
2221 {
2222  bool found = false;
2223  if(_ppDb && signatureId)
2224  {
2225  int rc = SQLITE_OK;
2226  sqlite3_stmt * ppStmt = 0;
2227  std::stringstream query;
2228 
2229  if(uStrNumCmp(_version, "0.18.0") >= 0)
2230  {
2231  query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps, env_sensors "
2232  "FROM Node "
2233  "WHERE id = " << signatureId <<
2234  ";";
2235  }
2236  else if(uStrNumCmp(_version, "0.14.0") >= 0)
2237  {
2238  query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps "
2239  "FROM Node "
2240  "WHERE id = " << signatureId <<
2241  ";";
2242  }
2243  else if(uStrNumCmp(_version, "0.13.0") >= 0)
2244  {
2245  query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
2246  "FROM Node "
2247  "WHERE id = " << signatureId <<
2248  ";";
2249  }
2250  else if(uStrNumCmp(_version, "0.11.1") >= 0)
2251  {
2252  query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
2253  "FROM Node "
2254  "WHERE id = " << signatureId <<
2255  ";";
2256  }
2257  else if(uStrNumCmp(_version, "0.8.5") >= 0)
2258  {
2259  query << "SELECT pose, map_id, weight, label, stamp "
2260  "FROM Node "
2261  "WHERE id = " << signatureId <<
2262  ";";
2263  }
2264  else
2265  {
2266  query << "SELECT pose, map_id, weight "
2267  "FROM Node "
2268  "WHERE id = " << signatureId <<
2269  ";";
2270  }
2271 
2272  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2273  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2274 
2275  const void * data = 0;
2276  int dataSize = 0;
2277 
2278  // Process the result if one
2279  rc = sqlite3_step(ppStmt);
2280  if(rc == SQLITE_ROW)
2281  {
2282  found = true;
2283  int index = 0;
2284  data = sqlite3_column_blob(ppStmt, index); // pose
2285  dataSize = sqlite3_column_bytes(ppStmt, index++);
2286  if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
2287  {
2288  memcpy(pose.data(), data, dataSize);
2289  if(uStrNumCmp(_version, "0.15.2") < 0)
2290  {
2291  pose.normalizeRotation();
2292  }
2293  }
2294 
2295  mapId = sqlite3_column_int(ppStmt, index++); // map id
2296  weight = sqlite3_column_int(ppStmt, index++); // weight
2297 
2298  if(uStrNumCmp(_version, "0.8.5") >= 0)
2299  {
2300  const unsigned char * p = sqlite3_column_text(ppStmt, index++);
2301  if(p)
2302  {
2303  label = reinterpret_cast<const char*>(p); // label
2304  }
2305  stamp = sqlite3_column_double(ppStmt, index++); // stamp
2306 
2307  if(uStrNumCmp(_version, "0.11.1") >= 0)
2308  {
2309  data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
2310  dataSize = sqlite3_column_bytes(ppStmt, index++);
2311  if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
2312  {
2313  memcpy(groundTruthPose.data(), data, dataSize);
2314  if(uStrNumCmp(_version, "0.15.2") < 0)
2315  {
2316  groundTruthPose.normalizeRotation();
2317  }
2318  }
2319 
2320  if(uStrNumCmp(_version, "0.13.0") >= 0)
2321  {
2322  velocity.resize(6,0);
2323  data = sqlite3_column_blob(ppStmt, index); // velocity
2324  dataSize = sqlite3_column_bytes(ppStmt, index++);
2325  if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
2326  {
2327  memcpy(velocity.data(), data, dataSize);
2328  }
2329  }
2330 
2331  if(uStrNumCmp(_version, "0.14.0") >= 0)
2332  {
2333  data = sqlite3_column_blob(ppStmt, index); // gps
2334  dataSize = sqlite3_column_bytes(ppStmt, index++);
2335  if((unsigned int)dataSize == 6*sizeof(double) && data)
2336  {
2337  const double * dataDouble = (const double *)data;
2338  gps = GPS(dataDouble[0], dataDouble[1], dataDouble[2], dataDouble[3], dataDouble[4], dataDouble[5]);
2339  }
2340 
2341  if(uStrNumCmp(_version, "0.18.0") >= 0)
2342  {
2343  data = sqlite3_column_blob(ppStmt, index);
2344  dataSize = sqlite3_column_bytes(ppStmt, index++);
2345  if(data)
2346  {
2347  UASSERT(dataSize%sizeof(double)==0 && (dataSize/sizeof(double))%3 == 0);
2348  const double * dataDouble = (const double *)data;
2349  int sensorsNum = (dataSize/sizeof(double))/3;
2350  for(int i=0; i<sensorsNum;++i)
2351  {
2352  EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
2353  sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
2354  }
2355  }
2356  }
2357  }
2358  }
2359  }
2360 
2361  rc = sqlite3_step(ppStmt); // next result...
2362  }
2363  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2364 
2365  // Finalize (delete) the statement
2366  rc = sqlite3_finalize(ppStmt);
2367  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2368  }
2369  return found;
2370 }
2371 
2372 void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
2373 {
2374  if(_ppDb)
2375  {
2376  UTimer timer;
2377  timer.start();
2378  int rc = SQLITE_OK;
2379  sqlite3_stmt * ppStmt = 0;
2380  std::string query;
2381 
2382  if(uStrNumCmp(_version, "0.11.11") >= 0)
2383  {
2384  query = "SELECT n.id "
2385  "FROM Node AS n "
2386  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
2387  "ORDER BY n.id;";
2388  }
2389  else
2390  {
2391  query = "SELECT n.id "
2392  "FROM Node AS n "
2393  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
2394  "ORDER BY n.id;";
2395  }
2396 
2397  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
2398  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2399 
2400  // Process the result if one
2401  rc = sqlite3_step(ppStmt);
2402  while(rc == SQLITE_ROW)
2403  {
2404  ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
2405  rc = sqlite3_step(ppStmt);
2406  }
2407  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2408 
2409  // Finalize (delete) the statement
2410  rc = sqlite3_finalize(ppStmt);
2411  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2412  ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
2413  }
2414 }
2415 
2416 void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const
2417 {
2418  if(_ppDb)
2419  {
2420  UTimer timer;
2421  timer.start();
2422  int rc = SQLITE_OK;
2423  sqlite3_stmt * ppStmt = 0;
2424  std::stringstream query;
2425 
2426  query << "SELECT DISTINCT id "
2427  << "FROM Node ";
2428  if(ignoreChildren)
2429  {
2430  query << "INNER JOIN Link ";
2431  query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
2432  query << "WHERE from_id != to_id "; // ignore self referring links
2433  query << "AND weight>-9 "; //ignore invalid nodes
2434  if(ignoreIntermediateNodes)
2435  {
2436  query << "AND weight!=-1 "; //ignore intermediate nodes
2437  }
2438  }
2439  else if(ignoreIntermediateNodes)
2440  {
2441  query << "WHERE weight!=-1 "; //ignore intermediate nodes
2442  }
2443 
2444  if(ignoreBadSignatures)
2445  {
2446  if(ignoreChildren || ignoreIntermediateNodes)
2447  {
2448  query << "AND ";
2449  }
2450  else
2451  {
2452  query << "WHERE ";
2453  }
2454  if(uStrNumCmp(_version, "0.13.0") >= 0)
2455  {
2456  query << " id in (select node_id from Feature) ";
2457  }
2458  else
2459  {
2460  query << " id in (select node_id from Map_Node_Word) ";
2461  }
2462  }
2463 
2464  query << "ORDER BY id";
2465 
2466  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2467  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2468 
2469 
2470  // Process the result if one
2471  rc = sqlite3_step(ppStmt);
2472  while(rc == SQLITE_ROW)
2473  {
2474  ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
2475  rc = sqlite3_step(ppStmt);
2476  }
2477  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2478 
2479  // Finalize (delete) the statement
2480  rc = sqlite3_finalize(ppStmt);
2481  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2482  ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
2483  }
2484 }
2485 
2486 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
2487 {
2488  links.clear();
2489  if(_ppDb)
2490  {
2491  UTimer timer;
2492  timer.start();
2493  int rc = SQLITE_OK;
2494  sqlite3_stmt * ppStmt = 0;
2495  std::stringstream query;
2496 
2497  if(uStrNumCmp(_version, "0.18.3") >= 0 && !withLandmarks)
2498  {
2499  query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link WHERE type!=" << Link::kLandmark << " ORDER BY from_id, to_id";
2500  }
2501  else if(uStrNumCmp(_version, "0.13.0") >= 0)
2502  {
2503  query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
2504  }
2505  else if(uStrNumCmp(_version, "0.10.10") >= 0)
2506  {
2507  query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
2508  }
2509  else if(uStrNumCmp(_version, "0.8.4") >= 0)
2510  {
2511  query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
2512  }
2513  else if(uStrNumCmp(_version, "0.7.4") >= 0)
2514  {
2515  query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
2516  }
2517  else
2518  {
2519  query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
2520  }
2521 
2522  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2523  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2524 
2525  int fromId = -1;
2526  int toId = -1;
2527  int type = Link::kUndef;
2528  const void * data = 0;
2529  int dataSize = 0;
2530 
2531  // Process the result if one
2532  rc = sqlite3_step(ppStmt);
2533  while(rc == SQLITE_ROW)
2534  {
2535  int index = 0;
2536 
2537  fromId = sqlite3_column_int(ppStmt, index++);
2538  toId = sqlite3_column_int(ppStmt, index++);
2539  type = sqlite3_column_int(ppStmt, index++);
2540 
2541  data = sqlite3_column_blob(ppStmt, index);
2542  dataSize = sqlite3_column_bytes(ppStmt, index++);
2543 
2544  Transform transform;
2545  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
2546  {
2547  memcpy(transform.data(), data, dataSize);
2548  if(uStrNumCmp(_version, "0.15.2") < 0)
2549  {
2550  transform.normalizeRotation();
2551  }
2552  }
2553  else if(dataSize)
2554  {
2555  UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
2556  }
2557 
2558  if(!ignoreNullLinks || !transform.isNull())
2559  {
2560  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
2561  if(uStrNumCmp(_version, "0.8.4") >= 0)
2562  {
2563  if(uStrNumCmp(_version, "0.13.0") >= 0)
2564  {
2565  data = sqlite3_column_blob(ppStmt, index);
2566  dataSize = sqlite3_column_bytes(ppStmt, index++);
2567  UASSERT(dataSize==36*sizeof(double) && data);
2568  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
2569  }
2570  else
2571  {
2572  double rotVariance = sqlite3_column_double(ppStmt, index++);
2573  double transVariance = sqlite3_column_double(ppStmt, index++);
2574  UASSERT(rotVariance > 0.0 && transVariance>0.0);
2575  informationMatrix.at<double>(0,0) = 1.0/transVariance;
2576  informationMatrix.at<double>(1,1) = 1.0/transVariance;
2577  informationMatrix.at<double>(2,2) = 1.0/transVariance;
2578  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
2579  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
2580  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
2581  }
2582 
2583  cv::Mat userDataCompressed;
2584  if(uStrNumCmp(_version, "0.10.10") >= 0)
2585  {
2586  const void * data = sqlite3_column_blob(ppStmt, index);
2587  dataSize = sqlite3_column_bytes(ppStmt, index++);
2588  //Create the userData
2589  if(dataSize>4 && data)
2590  {
2591  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
2592  }
2593  }
2594 
2595  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
2596  }
2597  else if(uStrNumCmp(_version, "0.7.4") >= 0)
2598  {
2599  double variance = sqlite3_column_double(ppStmt, index++);
2600  UASSERT(variance>0.0);
2601  informationMatrix *= 1.0/variance;
2602  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
2603  }
2604  else
2605  {
2606  // neighbor is 0, loop closures are 1 and 2 (child)
2607  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
2608  }
2609  }
2610 
2611  rc = sqlite3_step(ppStmt);
2612  }
2613 
2614  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2615 
2616  // Finalize (delete) the statement
2617  rc = sqlite3_finalize(ppStmt);
2618  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2619  }
2620 }
2621 
2622 void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName) const
2623 {
2624  if(_ppDb)
2625  {
2626  UDEBUG("get last %s from table \"%s\"", fieldName.c_str(), tableName.c_str());
2627  UTimer timer;
2628  timer.start();
2629  int rc = SQLITE_OK;
2630  sqlite3_stmt * ppStmt = 0;
2631  std::stringstream query;
2632 
2633  query << "SELECT COALESCE(MAX(" << fieldName << "), " << id << ") " // In case the table is empty, return back input id
2634  << "FROM " << tableName
2635  << ";";
2636 
2637  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2638  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2639 
2640 
2641  // Process the result if one
2642  rc = sqlite3_step(ppStmt);
2643  if(rc == SQLITE_ROW)
2644  {
2645  id = sqlite3_column_int(ppStmt, 0); // Signature Id
2646  rc = sqlite3_step(ppStmt);
2647  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2648  }
2649  else
2650  {
2651  UDEBUG("No result from the DB for table %s with field %s", tableName.c_str(), fieldName.c_str());
2652  }
2653 
2654  // Finalize (delete) the statement
2655  rc = sqlite3_finalize(ppStmt);
2656  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2657  ULOGGER_DEBUG("Time=%fs", timer.ticks());
2658  }
2659 }
2660 
2661 void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
2662 {
2663  ni = 0;
2664  if(_ppDb)
2665  {
2666  UTimer timer;
2667  timer.start();
2668  int rc = SQLITE_OK;
2669  sqlite3_stmt * ppStmt = 0;
2670  std::stringstream query;
2671 
2672  if(uStrNumCmp(_version, "0.13.0") >= 0)
2673  {
2674  query << "SELECT count(word_id) "
2675  << "FROM Feature "
2676  << "WHERE node_id=" << nodeId << ";";
2677  }
2678  else
2679  {
2680  query << "SELECT count(word_id) "
2681  << "FROM Map_Node_Word "
2682  << "WHERE node_id=" << nodeId << ";";
2683  }
2684 
2685  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2686  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2687 
2688 
2689  // Process the result if one
2690  rc = sqlite3_step(ppStmt);
2691  if(rc == SQLITE_ROW)
2692  {
2693  ni = sqlite3_column_int(ppStmt, 0);
2694  rc = sqlite3_step(ppStmt);
2695  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2696  }
2697  else
2698  {
2699  ULOGGER_ERROR("No result !?! from the DB, node=%d",nodeId);
2700  }
2701 
2702 
2703  // Finalize (delete) the statement
2704  rc = sqlite3_finalize(ppStmt);
2705  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2706  ULOGGER_DEBUG("Time=%fs", timer.ticks());
2707  }
2708 }
2709 
2710 void DBDriverSqlite3::getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const
2711 {
2712  if(_ppDb && landmarkId < 0 && uStrNumCmp(_version, "0.18.3") >= 0)
2713  {
2714  UTimer timer;
2715  timer.start();
2716  int rc = SQLITE_OK;
2717  sqlite3_stmt * ppStmt = 0;
2718  std::stringstream query;
2719 
2720  query << "SELECT from_id, type, information_matrix, transform, user_data FROM Link WHERE to_id=" << landmarkId <<"";
2721 
2722  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2723  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2724 
2725  // Process the result if one
2726  int fromId = -1;
2727  int linkType = -1;
2728  std::list<Link> links;
2729  const void * data = 0;
2730  int dataSize = 0;
2731 
2732  // Process the result if one
2733  rc = sqlite3_step(ppStmt);
2734  while(rc == SQLITE_ROW)
2735  {
2736  int index = 0;
2737 
2738  fromId = sqlite3_column_int(ppStmt, index++);
2739  linkType = sqlite3_column_int(ppStmt, index++);
2740  cv::Mat userDataCompressed;
2741  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
2742 
2743  data = sqlite3_column_blob(ppStmt, index);
2744  dataSize = sqlite3_column_bytes(ppStmt, index++);
2745  UASSERT(dataSize==36*sizeof(double) && data);
2746  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
2747 
2748  const void * data = sqlite3_column_blob(ppStmt, index);
2749  dataSize = sqlite3_column_bytes(ppStmt, index++);
2750  //Create the userData
2751  if(dataSize>4 && data)
2752  {
2753  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
2754  }
2755 
2756  //transform
2757  data = sqlite3_column_blob(ppStmt, index);
2758  dataSize = sqlite3_column_bytes(ppStmt, index++);
2759  Transform transform;
2760  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
2761  {
2762  memcpy(transform.data(), data, dataSize);
2763  }
2764  else if(dataSize)
2765  {
2766  UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, landmarkId);
2767  }
2768 
2769  if(linkType >= 0 && linkType != Link::kUndef)
2770  {
2771  nodes.insert(std::make_pair(fromId, Link(fromId, landmarkId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed)));
2772  }
2773  else
2774  {
2775  UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
2776  linkType, fromId, landmarkId);
2777  }
2778 
2779  rc = sqlite3_step(ppStmt);
2780  }
2781  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2782 
2783  // Finalize (delete) the statement
2784  rc = sqlite3_finalize(ppStmt);
2785  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2786  ULOGGER_DEBUG("Time=%f", timer.ticks());
2787  }
2788 }
2789 
2790 void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
2791 {
2792  if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
2793  {
2794  UTimer timer;
2795  timer.start();
2796  int rc = SQLITE_OK;
2797  sqlite3_stmt * ppStmt = 0;
2798  std::stringstream query;
2799  query << "SELECT id FROM Node WHERE label='" << label <<"'";
2800 
2801  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2802  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2803 
2804  // Process the result if one
2805  rc = sqlite3_step(ppStmt);
2806  if(rc == SQLITE_ROW)
2807  {
2808  id = sqlite3_column_int(ppStmt, 0);
2809  rc = sqlite3_step(ppStmt);
2810  }
2811  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2812 
2813  // Finalize (delete) the statement
2814  rc = sqlite3_finalize(ppStmt);
2815  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2816  ULOGGER_DEBUG("Time=%f", timer.ticks());
2817  }
2818 }
2819 
2820 void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
2821 {
2822  if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
2823  {
2824  UTimer timer;
2825  timer.start();
2826  int rc = SQLITE_OK;
2827  sqlite3_stmt * ppStmt = 0;
2828  std::stringstream query;
2829  query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
2830 
2831  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2832  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2833 
2834  // Process the result if one
2835  rc = sqlite3_step(ppStmt);
2836  while(rc == SQLITE_ROW)
2837  {
2838  int index = 0;
2839  int id = sqlite3_column_int(ppStmt, index++);
2840  const unsigned char * p = sqlite3_column_text(ppStmt, index++);
2841  if(p)
2842  {
2843  std::string label = reinterpret_cast<const char*>(p);
2844  if(!label.empty())
2845  {
2846  labels.insert(std::make_pair(id, label));
2847  }
2848  }
2849  rc = sqlite3_step(ppStmt);
2850  }
2851  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2852 
2853  // Finalize (delete) the statement
2854  rc = sqlite3_finalize(ppStmt);
2855  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2856  ULOGGER_DEBUG("Time=%f", timer.ticks());
2857  }
2858 }
2859 
2860 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
2861 {
2862  weight = 0;
2863  if(_ppDb)
2864  {
2865  UTimer timer;
2866  timer.start();
2867  int rc = SQLITE_OK;
2868  sqlite3_stmt * ppStmt = 0;
2869  std::stringstream query;
2870 
2871  query << "SELECT weight FROM node WHERE id = "
2872  << nodeId
2873  << ";";
2874 
2875  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2876  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2877 
2878 
2879  // Process the result if one
2880  rc = sqlite3_step(ppStmt);
2881  if(rc == SQLITE_ROW)
2882  {
2883  weight= sqlite3_column_int(ppStmt, 0); // weight
2884  rc = sqlite3_step(ppStmt);
2885  }
2886  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2887 
2888  // Finalize (delete) the statement
2889  rc = sqlite3_finalize(ppStmt);
2890  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2891  }
2892 }
2893 
2894 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
2895 void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
2896 {
2897  ULOGGER_DEBUG("count=%d", (int)ids.size());
2898  if(_ppDb && ids.size())
2899  {
2900  std::string type;
2901  UTimer timer;
2902  timer.start();
2903  int rc = SQLITE_OK;
2904  sqlite3_stmt * ppStmt = 0;
2905  std::stringstream query;
2906  unsigned int loaded = 0;
2907 
2908  // Load nodes information
2909  if(uStrNumCmp(_version, "0.18.0") >= 0)
2910  {
2911  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors "
2912  << "FROM Node "
2913  << "WHERE id=?;";
2914  }
2915  else if(uStrNumCmp(_version, "0.14.0") >= 0)
2916  {
2917  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps "
2918  << "FROM Node "
2919  << "WHERE id=?;";
2920  }
2921  else if(uStrNumCmp(_version, "0.13.0") >= 0)
2922  {
2923  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
2924  << "FROM Node "
2925  << "WHERE id=?;";
2926  }
2927  else if(uStrNumCmp(_version, "0.11.1") >= 0)
2928  {
2929  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
2930  << "FROM Node "
2931  << "WHERE id=?;";
2932  }
2933  else if(uStrNumCmp(_version, "0.8.5") >= 0)
2934  {
2935  query << "SELECT id, map_id, weight, pose, stamp, label "
2936  << "FROM Node "
2937  << "WHERE id=?;";
2938  }
2939  else
2940  {
2941  query << "SELECT id, map_id, weight, pose "
2942  << "FROM Node "
2943  << "WHERE id=?;";
2944  }
2945 
2946  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2947  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2948 
2949  for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
2950  {
2951  //ULOGGER_DEBUG("Loading node %d...", *iter);
2952  // bind id
2953  rc = sqlite3_bind_int(ppStmt, 1, *iter);
2954  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2955 
2956  int id = 0;
2957  int mapId = 0;
2958  double stamp = 0.0;
2959  int weight = 0;
2960  Transform pose;
2961  Transform groundTruthPose;
2962  std::vector<float> velocity;
2963  std::vector<double> gps;
2964  EnvSensors sensors;
2965  const void * data = 0;
2966  int dataSize = 0;
2967  std::string label;
2968 
2969  // Process the result if one
2970  rc = sqlite3_step(ppStmt);
2971  if(rc == SQLITE_ROW)
2972  {
2973  int index = 0;
2974  id = sqlite3_column_int(ppStmt, index++); // Signature Id
2975  mapId = sqlite3_column_int(ppStmt, index++); // Map Id
2976  weight = sqlite3_column_int(ppStmt, index++); // weight
2977 
2978  data = sqlite3_column_blob(ppStmt, index); // pose
2979  dataSize = sqlite3_column_bytes(ppStmt, index++);
2980  if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
2981  {
2982  memcpy(pose.data(), data, dataSize);
2983  if(uStrNumCmp(_version, "0.15.2") < 0)
2984  {
2985  pose.normalizeRotation();
2986  }
2987  }
2988 
2989  if(uStrNumCmp(_version, "0.8.5") >= 0)
2990  {
2991  stamp = sqlite3_column_double(ppStmt, index++); // stamp
2992  const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
2993  if(p)
2994  {
2995  label = reinterpret_cast<const char*>(p);
2996  }
2997 
2998  if(uStrNumCmp(_version, "0.11.1") >= 0)
2999  {
3000  data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
3001  dataSize = sqlite3_column_bytes(ppStmt, index++);
3002  if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
3003  {
3004  memcpy(groundTruthPose.data(), data, dataSize);
3005  if(uStrNumCmp(_version, "0.15.2") < 0)
3006  {
3007  groundTruthPose.normalizeRotation();
3008  }
3009  }
3010 
3011  if(uStrNumCmp(_version, "0.13.0") >= 0)
3012  {
3013  velocity.resize(6,0);
3014  data = sqlite3_column_blob(ppStmt, index); // velocity
3015  dataSize = sqlite3_column_bytes(ppStmt, index++);
3016  if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
3017  {
3018  memcpy(velocity.data(), data, dataSize);
3019  }
3020  }
3021 
3022  if(uStrNumCmp(_version, "0.14.0") >= 0)
3023  {
3024  gps.resize(6,0);
3025  data = sqlite3_column_blob(ppStmt, index); // gps
3026  dataSize = sqlite3_column_bytes(ppStmt, index++);
3027  if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
3028  {
3029  memcpy(gps.data(), data, dataSize);
3030  }
3031 
3032  if(uStrNumCmp(_version, "0.18.0") >= 0)
3033  {
3034  data = sqlite3_column_blob(ppStmt, index); // env_sensors
3035  dataSize = sqlite3_column_bytes(ppStmt, index++);
3036  if(data)
3037  {
3038  UASSERT(dataSize%sizeof(double)==0 && (dataSize/sizeof(double))%3 == 0);
3039  const double * dataDouble = (const double *)data;
3040  int sensorsNum = (dataSize/sizeof(double))/3;
3041  for(int i=0; i<sensorsNum;++i)
3042  {
3043  EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
3044  sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
3045  }
3046  }
3047  }
3048  }
3049  }
3050  }
3051 
3052  rc = sqlite3_step(ppStmt);
3053  }
3054  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3055 
3056  // create the node
3057  if(id)
3058  {
3059  ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
3060  Signature * s = new Signature(
3061  id,
3062  mapId,
3063  weight,
3064  stamp,
3065  label,
3066  pose,
3067  groundTruthPose);
3068  if(velocity.size() == 6)
3069  {
3070  s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
3071  }
3072  if(gps.size() == 6)
3073  {
3074  s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
3075  }
3076  s->sensorData().setEnvSensors(sensors);
3077  s->setSaved(true);
3078  nodes.push_back(s);
3079  ++loaded;
3080  }
3081  else
3082  {
3083  UERROR("Signature %d not found in database!", *iter);
3084  }
3085 
3086  //reset
3087  rc = sqlite3_reset(ppStmt);
3088  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3089  }
3090 
3091  // Finalize (delete) the statement
3092  rc = sqlite3_finalize(ppStmt);
3093  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3094 
3095  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3096 
3097  // Prepare the query... Get the map from signature and visual words
3098  std::stringstream query2;
3099  if(uStrNumCmp(_version, "0.13.0") >= 0)
3100  {
3101  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3102  "FROM Feature "
3103  "WHERE node_id = ? ";
3104  }
3105  else if(uStrNumCmp(_version, "0.12.0") >= 0)
3106  {
3107  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3108  "FROM Map_Node_Word "
3109  "WHERE node_id = ? ";
3110  }
3111  else if(uStrNumCmp(_version, "0.11.2") >= 0)
3112  {
3113  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3114  "FROM Map_Node_Word "
3115  "WHERE node_id = ? ";
3116  }
3117  else
3118  {
3119  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
3120  "FROM Map_Node_Word "
3121  "WHERE node_id = ? ";
3122  }
3123 
3124  query2 << " ORDER BY word_id"; // Needed for fast insertion below
3125  query2 << ";";
3126 
3127  rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
3128  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3129 
3130  float nanFloat = std::numeric_limits<float>::quiet_NaN ();
3131 
3132  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3133  {
3134  //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
3135  // bind id
3136  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3137  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3138 
3139  int visualWordId = 0;
3140  int descriptorSize = 0;
3141  const void * descriptor = 0;
3142  int dRealSize = 0;
3143  cv::KeyPoint kpt;
3144  std::multimap<int, int> visualWords;
3145  std::vector<cv::KeyPoint> visualWordsKpts;
3146  std::vector<cv::Point3f> visualWords3;
3147  cv::Mat descriptors;
3148  bool allWords3NaN = true;
3149  cv::Point3f depth(0,0,0);
3150 
3151  // Process the result if one
3152  rc = sqlite3_step(ppStmt);
3153  while(rc == SQLITE_ROW)
3154  {
3155  int index = 0;
3156  visualWordId = sqlite3_column_int(ppStmt, index++);
3157  kpt.pt.x = sqlite3_column_double(ppStmt, index++);
3158  kpt.pt.y = sqlite3_column_double(ppStmt, index++);
3159  kpt.size = sqlite3_column_int(ppStmt, index++);
3160  kpt.angle = sqlite3_column_double(ppStmt, index++);
3161  kpt.response = sqlite3_column_double(ppStmt, index++);
3162  if(uStrNumCmp(_version, "0.12.0") >= 0)
3163  {
3164  kpt.octave = sqlite3_column_int(ppStmt, index++);
3165  }
3166 
3167  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3168  {
3169  depth.x = nanFloat;
3170  ++index;
3171  }
3172  else
3173  {
3174  depth.x = sqlite3_column_double(ppStmt, index++);
3175  }
3176 
3177  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3178  {
3179  depth.y = nanFloat;
3180  ++index;
3181  }
3182  else
3183  {
3184  depth.y = sqlite3_column_double(ppStmt, index++);
3185  }
3186 
3187  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3188  {
3189  depth.z = nanFloat;
3190  ++index;
3191  }
3192  else
3193  {
3194  depth.z = sqlite3_column_double(ppStmt, index++);
3195  }
3196 
3197  visualWordsKpts.push_back(kpt);
3198  visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
3199  visualWords3.push_back(depth);
3200 
3201  if(allWords3NaN && util3d::isFinite(depth))
3202  {
3203  allWords3NaN = false;
3204  }
3205 
3206  if(uStrNumCmp(_version, "0.11.2") >= 0)
3207  {
3208  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3209  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3210  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3211 
3212  if(descriptor && descriptorSize>0 && dRealSize>0)
3213  {
3214  cv::Mat d;
3215  if(dRealSize == descriptorSize)
3216  {
3217  // CV_8U binary descriptors
3218  d = cv::Mat(1, descriptorSize, CV_8U);
3219  }
3220  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3221  {
3222  // CV_32F
3223  d = cv::Mat(1, descriptorSize, CV_32F);
3224  }
3225  else
3226  {
3227  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3228  }
3229 
3230  memcpy(d.data, descriptor, dRealSize);
3231 
3232  descriptors.push_back(d);
3233  }
3234  }
3235 
3236  rc = sqlite3_step(ppStmt);
3237  }
3238  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3239 
3240  if(visualWords.size()==0)
3241  {
3242  UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
3243  }
3244  else
3245  {
3246  if(allWords3NaN)
3247  {
3248  visualWords3.clear();
3249  }
3250  (*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
3251  ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
3252  }
3253 
3254  //reset
3255  rc = sqlite3_reset(ppStmt);
3256  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3257  }
3258 
3259  // Finalize (delete) the statement
3260  rc = sqlite3_finalize(ppStmt);
3261  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3262 
3263  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3264 
3265  this->loadLinksQuery(nodes);
3266  ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
3267 
3268  for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
3269  {
3270  (*iter)->setModified(false);
3271  }
3272 
3273  // load calibrations
3274  if(nodes.size() && uStrNumCmp(_version, "0.10.0") >= 0)
3275  {
3276  std::stringstream query3;
3277  query3 << "SELECT calibration "
3278  "FROM Data "
3279  "WHERE id = ? ";
3280 
3281  rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
3282  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3283 
3284  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3285  {
3286  // bind id
3287  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3288  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3289 
3290  rc = sqlite3_step(ppStmt);
3291  if(rc == SQLITE_ROW)
3292  {
3293  int index=0;
3294  const void * data = 0;
3295  int dataSize = 0;
3296  Transform localTransform;
3297  std::vector<CameraModel> models;
3298  std::vector<StereoCameraModel> stereoModels;
3299 
3300  // calibration
3301  data = sqlite3_column_blob(ppStmt, index);
3302  dataSize = sqlite3_column_bytes(ppStmt, index++);
3303  // multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
3304  // stereo [fx, fy, cx, cy, baseline, [width,height], local_transform] (5or7+12)*float
3305  if(dataSize > 0 && data)
3306  {
3307  if(uStrNumCmp(_version, "0.18.0") >= 0)
3308  {
3309  if(dataSize >= int(sizeof(int)*4))
3310  {
3311  const int * dataInt = (const int *)data;
3312  int type = dataInt[3];
3313  if(type == 0) // mono
3314  {
3315  CameraModel model;
3316  int bytesReadTotal = 0;
3317  unsigned int bytesRead = 0;
3318  while(bytesReadTotal < dataSize &&
3319  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
3320  {
3321  bytesReadTotal+=bytesRead;
3322  models.push_back(model);
3323  }
3324  UASSERT(bytesReadTotal == dataSize);
3325  }
3326  else if(type == 1) // stereo
3327  {
3328  StereoCameraModel model;
3329  int bytesReadTotal = 0;
3330  unsigned int bytesRead = 0;
3331  while(bytesReadTotal < dataSize &&
3332  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
3333  {
3334  bytesReadTotal+=bytesRead;
3335  stereoModels.push_back(model);
3336  }
3337  UASSERT(bytesReadTotal == dataSize);
3338  }
3339  else
3340  {
3341  UFATAL("Unknown calibration type %d", type);
3342  }
3343  }
3344  else
3345  {
3346  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
3347  }
3348  }
3349  else
3350  {
3351  float * dataFloat = (float*)data;
3352  if(uStrNumCmp(_version, "0.11.2") >= 0 &&
3353  (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
3354  {
3355  int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
3356  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
3357  int max = cameraCount*(6+localTransform.size());
3358  for(int i=0; i<max; i+=6+localTransform.size())
3359  {
3360  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
3361  localTransform = Transform::getIdentity();
3362  memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
3363  if(uStrNumCmp(_version, "0.15.2") < 0)
3364  {
3365  localTransform.normalizeRotation();
3366  }
3367  models.push_back(CameraModel(
3368  (double)dataFloat[i],
3369  (double)dataFloat[i+1],
3370  (double)dataFloat[i+2],
3371  (double)dataFloat[i+3],
3372  localTransform,
3373  0,
3374  cv::Size(dataFloat[i+4], dataFloat[i+5])));
3375  UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
3376  dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
3377  localTransform.prettyPrint().c_str());
3378  }
3379  }
3380  else if(uStrNumCmp(_version, "0.11.2") < 0 &&
3381  (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
3382  {
3383  int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
3384  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
3385  int max = cameraCount*(4+localTransform.size());
3386  for(int i=0; i<max; i+=4+localTransform.size())
3387  {
3388  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
3389  localTransform = Transform::getIdentity();
3390  memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
3391  if(uStrNumCmp(_version, "0.15.2") < 0)
3392  {
3393  localTransform.normalizeRotation();
3394  }
3395  models.push_back(CameraModel(
3396  (double)dataFloat[i],
3397  (double)dataFloat[i+1],
3398  (double)dataFloat[i+2],
3399  (double)dataFloat[i+3],
3400  localTransform));
3401  }
3402  }
3403  else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
3404  {
3405  UDEBUG("Loading calibration of a stereo camera");
3406  memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
3407  if(uStrNumCmp(_version, "0.15.2") < 0)
3408  {
3409  localTransform.normalizeRotation();
3410  }
3411  stereoModels.push_back(StereoCameraModel(
3412  dataFloat[0], // fx
3413  dataFloat[1], // fy
3414  dataFloat[2], // cx
3415  dataFloat[3], // cy
3416  dataFloat[4], // baseline
3417  localTransform,
3418  cv::Size(dataFloat[5], dataFloat[6])));
3419  }
3420  else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
3421  {
3422  UDEBUG("Loading calibration of a stereo camera");
3423  memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
3424  if(uStrNumCmp(_version, "0.15.2") < 0)
3425  {
3426  localTransform.normalizeRotation();
3427  }
3428  stereoModels.push_back(StereoCameraModel(
3429  dataFloat[0], // fx
3430  dataFloat[1], // fy
3431  dataFloat[2], // cx
3432  dataFloat[3], // cy
3433  dataFloat[4], // baseline
3434  localTransform));
3435  }
3436  else
3437  {
3438  UFATAL("Wrong format of the Data.calibration field (size=%d bytes, db version=%s)", dataSize, _version.c_str());
3439  }
3440  }
3441 
3442  (*iter)->sensorData().setCameraModels(models);
3443  (*iter)->sensorData().setStereoCameraModels(stereoModels);
3444  }
3445  rc = sqlite3_step(ppStmt);
3446  }
3447  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3448 
3449  //reset
3450  rc = sqlite3_reset(ppStmt);
3451  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3452  }
3453  // Finalize (delete) the statement
3454  rc = sqlite3_finalize(ppStmt);
3455  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3456 
3457  ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
3458  }
3459 
3460  // load global descriptors
3461  if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
3462  {
3463  std::stringstream query3;
3464  query3 << "SELECT type, info, data "
3465  "FROM GlobalDescriptor "
3466  "WHERE node_id = ? ";
3467 
3468  rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
3469  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3470 
3471  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3472  {
3473  // bind id
3474  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3475  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3476 
3477  std::vector<GlobalDescriptor> globalDescriptors;
3478 
3479  rc = sqlite3_step(ppStmt);
3480  while(rc == SQLITE_ROW)
3481  {
3482  int index=0;
3483  const void * data = 0;
3484  int dataSize = 0;
3485  int type = -1;
3486  cv::Mat info;
3487  cv::Mat dataMat;
3488 
3489  type = sqlite3_column_int(ppStmt, index++);
3490  data = sqlite3_column_blob(ppStmt, index);
3491  dataSize = sqlite3_column_bytes(ppStmt, index++);
3492  if(dataSize && data)
3493  {
3494  info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
3495  }
3496  data = sqlite3_column_blob(ppStmt, index);
3497  dataSize = sqlite3_column_bytes(ppStmt, index++);
3498  if(dataSize && data)
3499  {
3500  dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
3501  }
3502 
3503  UASSERT(!dataMat.empty());
3504  globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
3505 
3506  rc = sqlite3_step(ppStmt);
3507  }
3508  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3509 
3510  if(!globalDescriptors.empty())
3511  {
3512  (*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
3513  ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
3514  }
3515 
3516  //reset
3517  rc = sqlite3_reset(ppStmt);
3518  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3519  }
3520  // Finalize (delete) the statement
3521  rc = sqlite3_finalize(ppStmt);
3522  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3523 
3524  ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
3525  }
3526 
3527  if(ids.size() != loaded)
3528  {
3529  UERROR("Some signatures not found in database");
3530  }
3531  }
3532 }
3533 
3534 void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
3535 {
3536  ULOGGER_DEBUG("");
3537  if(_ppDb)
3538  {
3539  std::string type;
3540  UTimer timer;
3541  timer.start();
3542  int rc = SQLITE_OK;
3543  sqlite3_stmt * ppStmt = 0;
3544  std::string query;
3545  std::list<int> ids;
3546 
3547  if(uStrNumCmp(_version, "0.11.11") >= 0)
3548  {
3549  query = "SELECT n.id "
3550  "FROM Node AS n "
3551  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
3552  "ORDER BY n.id;";
3553  }
3554  else
3555  {
3556  query = "SELECT n.id "
3557  "FROM Node AS n "
3558  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
3559  "ORDER BY n.id;";
3560  }
3561 
3562  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
3563  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3564 
3565  // Process the result if one
3566  rc = sqlite3_step(ppStmt);
3567  while(rc == SQLITE_ROW)
3568  {
3569  ids.push_back(sqlite3_column_int(ppStmt, 0)); // Signature id
3570  rc = sqlite3_step(ppStmt); // next result...
3571  }
3572 
3573  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3574 
3575  // Finalize (delete) the statement
3576  rc = sqlite3_finalize(ppStmt);
3577  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3578 
3579  ULOGGER_DEBUG("Loading %d signatures...", ids.size());
3580  this->loadSignaturesQuery(ids, nodes);
3581  ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
3582  }
3583 }
3584 
3585 void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
3586 {
3587  ULOGGER_DEBUG("");
3588  if(_ppDb && dictionary)
3589  {
3590  std::string type;
3591  UTimer timer;
3592  timer.start();
3593  int rc = SQLITE_OK;
3594  sqlite3_stmt * ppStmt = 0;
3595  std::stringstream query;
3596  std::list<VisualWord *> visualWords;
3597 
3598  // Get the visual words
3599  query << "SELECT id, descriptor_size, descriptor FROM Word ";
3600  if(lastStateOnly)
3601  {
3602  if(uStrNumCmp(_version, "0.11.11") >= 0)
3603  {
3604  query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
3605  }
3606  else
3607  {
3608  query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
3609  }
3610  }
3611  query << "ORDER BY id;";
3612 
3613  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3614  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3615 
3616  // Process the result if one
3617  int id = 0;
3618  int descriptorSize = 0;
3619  const void * descriptor = 0;
3620  int dRealSize = 0;
3621  rc = sqlite3_step(ppStmt);
3622  int count = 0;
3623  while(rc == SQLITE_ROW)
3624  {
3625  int index=0;
3626  id = sqlite3_column_int(ppStmt, index++); // VisualWord Id
3627 
3628  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3629  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3630  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3631 
3632  cv::Mat d;
3633  if(dRealSize == descriptorSize)
3634  {
3635  // CV_8U binary descriptors
3636  d = cv::Mat(1, descriptorSize, CV_8U);
3637  }
3638  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3639  {
3640  // CV_32F
3641  d = cv::Mat(1, descriptorSize, CV_32F);
3642  }
3643  else
3644  {
3645  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3646  }
3647 
3648  memcpy(d.data, descriptor, dRealSize);
3649  VisualWord * vw = new VisualWord(id, d);
3650  vw->setSaved(true);
3651  dictionary->addWord(vw);
3652 
3653  if(++count % 5000 == 0)
3654  {
3655  ULOGGER_DEBUG("Loaded %d words...", count);
3656  }
3657  rc = sqlite3_step(ppStmt); // next result...
3658  }
3659  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3660  // Finalize (delete) the statement
3661  rc = sqlite3_finalize(ppStmt);
3662  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3663 
3664  // Get Last word id
3665  getLastWordId(id);
3666  dictionary->setLastWordId(id);
3667 
3668  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3669  }
3670 }
3671 
3672 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
3673 void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
3674 {
3675  ULOGGER_DEBUG("size=%d", wordIds.size());
3676  if(_ppDb && wordIds.size())
3677  {
3678  std::string type;
3679  UTimer timer;
3680  timer.start();
3681  int rc = SQLITE_OK;
3682  sqlite3_stmt * ppStmt = 0;
3683  std::stringstream query;
3684  std::set<int> loaded;
3685 
3686  // Get the map from signature and visual words
3687  query << "SELECT vw.descriptor_size, vw.descriptor "
3688  "FROM Word as vw "
3689  "WHERE vw.id = ?;";
3690 
3691  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3692  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3693 
3694  int descriptorSize;
3695  const void * descriptor;
3696  int dRealSize;
3697  unsigned long dRealSizeTotal = 0;
3698  for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
3699  {
3700  // bind id
3701  rc = sqlite3_bind_int(ppStmt, 1, *iter);
3702  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3703 
3704  // Process the result if one
3705  rc = sqlite3_step(ppStmt);
3706  if(rc == SQLITE_ROW)
3707  {
3708  int index = 0;
3709  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3710  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3711  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3712 
3713  cv::Mat d;
3714  if(dRealSize == descriptorSize)
3715  {
3716  // CV_8U binary descriptors
3717  d = cv::Mat(1, descriptorSize, CV_8U);
3718  }
3719  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3720  {
3721  // CV_32F
3722  d = cv::Mat(1, descriptorSize, CV_32F);
3723  }
3724  else
3725  {
3726  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3727  }
3728 
3729  memcpy(d.data, descriptor, dRealSize);
3730  dRealSizeTotal+=dRealSize;
3731  VisualWord * vw = new VisualWord(*iter, d);
3732  if(vw)
3733  {
3734  vw->setSaved(true);
3735  }
3736  vws.push_back(vw);
3737  loaded.insert(loaded.end(), *iter);
3738 
3739  rc = sqlite3_step(ppStmt);
3740  }
3741 
3742  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3743 
3744  rc = sqlite3_reset(ppStmt);
3745  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3746  }
3747 
3748  // Finalize (delete) the statement
3749  rc = sqlite3_finalize(ppStmt);
3750  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3751 
3752  UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);
3753 
3754  if(wordIds.size() != loaded.size())
3755  {
3756  for(std::set<int>::const_iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
3757  {
3758  if(loaded.find(*iter) == loaded.end())
3759  {
3760  UDEBUG("Not found word %d", *iter);
3761  }
3762  }
3763  UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
3764  }
3765  }
3766 }
3767 
3768 void DBDriverSqlite3::loadLinksQuery(
3769  int signatureId,
3770  std::multimap<int, Link> & links,
3771  Link::Type typeIn) const
3772 {
3773  links.clear();
3774  if(_ppDb)
3775  {
3776  UTimer timer;
3777  timer.start();
3778  int rc = SQLITE_OK;
3779  sqlite3_stmt * ppStmt = 0;
3780  std::stringstream query;
3781 
3782  if(uStrNumCmp(_version, "0.13.0") >= 0)
3783  {
3784  query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
3785  }
3786  else if(uStrNumCmp(_version, "0.10.10") >= 0)
3787  {
3788  query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
3789  }
3790  else if(uStrNumCmp(_version, "0.8.4") >= 0)
3791  {
3792  query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
3793  }
3794  else if(uStrNumCmp(_version, "0.7.4") >= 0)
3795  {
3796  query << "SELECT to_id, type, transform, variance FROM Link ";
3797  }
3798  else
3799  {
3800  query << "SELECT to_id, type, transform FROM Link ";
3801  }
3802  query << "WHERE from_id = " << signatureId;
3803  if(typeIn < Link::kEnd)
3804  {
3805  if(uStrNumCmp(_version, "0.7.4") >= 0)
3806  {
3807  query << " AND type = " << typeIn;
3808  }
3809  else if(typeIn == Link::kNeighbor)
3810  {
3811  query << " AND type = 0";
3812  }
3813  else if(typeIn > Link::kNeighbor)
3814  {
3815  query << " AND type > 0";
3816  }
3817  }
3818  if(uStrNumCmp(_version, "0.18.3") >= 0 && (typeIn != Link::kAllWithLandmarks && typeIn != Link::kLandmark))
3819  {
3820  query << " AND type != " << Link::kLandmark;
3821  }
3822 
3823  query << " ORDER BY to_id";
3824 
3825  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3826  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3827 
3828  int toId = -1;
3829  int type = Link::kUndef;
3830  const void * data = 0;
3831  int dataSize = 0;
3832 
3833  // Process the result if one
3834  rc = sqlite3_step(ppStmt);
3835  while(rc == SQLITE_ROW)
3836  {
3837  int index = 0;
3838 
3839  toId = sqlite3_column_int(ppStmt, index++);
3840  type = sqlite3_column_int(ppStmt, index++);
3841 
3842  data = sqlite3_column_blob(ppStmt, index);
3843  dataSize = sqlite3_column_bytes(ppStmt, index++);
3844 
3845  Transform transform;
3846  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
3847  {
3848  memcpy(transform.data(), data, dataSize);
3849  if(uStrNumCmp(_version, "0.15.2") < 0)
3850  {
3851  transform.normalizeRotation();
3852  }
3853  }
3854  else if(dataSize)
3855  {
3856  UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
3857  }
3858 
3859  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
3860  if(uStrNumCmp(_version, "0.8.4") >= 0)
3861  {
3862  if(uStrNumCmp(_version, "0.13.0") >= 0)
3863  {
3864  data = sqlite3_column_blob(ppStmt, index);
3865  dataSize = sqlite3_column_bytes(ppStmt, index++);
3866  UASSERT(dataSize==36*sizeof(double) && data);
3867  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
3868  }
3869  else
3870  {
3871  double rotVariance = sqlite3_column_double(ppStmt, index++);
3872  double transVariance = sqlite3_column_double(ppStmt, index++);
3873  UASSERT(rotVariance > 0.0 && transVariance>0.0);
3874  informationMatrix.at<double>(0,0) = 1.0/transVariance;
3875  informationMatrix.at<double>(1,1) = 1.0/transVariance;
3876  informationMatrix.at<double>(2,2) = 1.0/transVariance;
3877  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
3878  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
3879  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
3880  }
3881 
3882  cv::Mat userDataCompressed;
3883  if(uStrNumCmp(_version, "0.10.10") >= 0)
3884  {
3885  const void * data = sqlite3_column_blob(ppStmt, index);
3886  dataSize = sqlite3_column_bytes(ppStmt, index++);
3887  //Create the userData
3888  if(dataSize>4 && data)
3889  {
3890  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
3891  }
3892  }
3893 
3894  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
3895  }
3896  else if(uStrNumCmp(_version, "0.7.4") >= 0)
3897  {
3898  double variance = sqlite3_column_double(ppStmt, index++);
3899  UASSERT(variance>0.0);
3900  informationMatrix *= 1.0/variance;
3901  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
3902  }
3903  else
3904  {
3905  // neighbor is 0, loop closures are 1
3906  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
3907  }
3908 
3909  rc = sqlite3_step(ppStmt);
3910  }
3911 
3912  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3913 
3914  // Finalize (delete) the statement
3915  rc = sqlite3_finalize(ppStmt);
3916  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3917 
3918  if(links.size() == 0)
3919  {
3920  //UERROR("No links loaded from signature %d", signatureId);
3921  }
3922  }
3923 }
3924 
3925 void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
3926 {
3927  if(_ppDb)
3928  {
3929  UTimer timer;
3930  timer.start();
3931  int rc = SQLITE_OK;
3932  sqlite3_stmt * ppStmt = 0;
3933  std::stringstream query;
3934 
3935  if(uStrNumCmp(_version, "0.13.0") >= 0)
3936  {
3937  query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
3938  << "WHERE from_id = ? "
3939  << "ORDER BY to_id";
3940  }
3941  else if(uStrNumCmp(_version, "0.10.10") >= 0)
3942  {
3943  query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
3944  << "WHERE from_id = ? "
3945  << "ORDER BY to_id";
3946  }
3947  else if(uStrNumCmp(_version, "0.8.4") >= 0)
3948  {
3949  query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
3950  << "WHERE from_id = ? "
3951  << "ORDER BY to_id";
3952  }
3953  else if(uStrNumCmp(_version, "0.7.4") >= 0)
3954  {
3955  query << "SELECT to_id, type, variance, transform FROM Link "
3956  << "WHERE from_id = ? "
3957  << "ORDER BY to_id";
3958  }
3959  else
3960  {
3961  query << "SELECT to_id, type, transform FROM Link "
3962  << "WHERE from_id = ? "
3963  << "ORDER BY to_id";
3964  }
3965 
3966  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3967  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3968 
3969  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
3970  {
3971  // bind id
3972  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3973  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3974 
3975  int toId = -1;
3976  int linkType = -1;
3977  std::list<Link> links;
3978  const void * data = 0;
3979  int dataSize = 0;
3980 
3981  // Process the result if one
3982  rc = sqlite3_step(ppStmt);
3983  while(rc == SQLITE_ROW)
3984  {
3985  int index = 0;
3986 
3987  toId = sqlite3_column_int(ppStmt, index++);
3988  linkType = sqlite3_column_int(ppStmt, index++);
3989  cv::Mat userDataCompressed;
3990  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
3991  if(uStrNumCmp(_version, "0.8.4") >= 0)
3992  {
3993  if(uStrNumCmp(_version, "0.13.0") >= 0)
3994  {
3995  data = sqlite3_column_blob(ppStmt, index);
3996  dataSize = sqlite3_column_bytes(ppStmt, index++);
3997  UASSERT(dataSize==36*sizeof(double) && data);
3998  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
3999  }
4000  else
4001  {
4002  double rotVariance = sqlite3_column_double(ppStmt, index++);
4003  double transVariance = sqlite3_column_double(ppStmt, index++);
4004  UASSERT(rotVariance > 0.0 && transVariance>0.0);
4005  informationMatrix.at<double>(0,0) = 1.0/transVariance;
4006  informationMatrix.at<double>(1,1) = 1.0/transVariance;
4007  informationMatrix.at<double>(2,2) = 1.0/transVariance;
4008  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
4009  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
4010  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
4011  }
4012 
4013  if(uStrNumCmp(_version, "0.10.10") >= 0)
4014  {
4015  const void * data = sqlite3_column_blob(ppStmt, index);
4016  dataSize = sqlite3_column_bytes(ppStmt, index++);
4017  //Create the userData
4018  if(dataSize>4 && data)
4019  {
4020  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
4021  }
4022  }
4023  }
4024  else if(uStrNumCmp(_version, "0.7.4") >= 0)
4025  {
4026  double variance = sqlite3_column_double(ppStmt, index++);
4027  UASSERT(variance>0.0);
4028  informationMatrix *= 1.0/variance;
4029  }
4030 
4031  //transform
4032  data = sqlite3_column_blob(ppStmt, index);
4033  dataSize = sqlite3_column_bytes(ppStmt, index++);
4034  Transform transform;
4035  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
4036  {
4037  memcpy(transform.data(), data, dataSize);
4038  if(uStrNumCmp(_version, "0.15.2") < 0)
4039  {
4040  transform.normalizeRotation();
4041  }
4042  }
4043  else if(dataSize)
4044  {
4045  UERROR("Error while loading link transform from %d to %d! Setting to null...", (*iter)->id(), toId);
4046  }
4047 
4048  if(linkType >= 0 && linkType != Link::kUndef)
4049  {
4050  if(linkType == Link::kLandmark)
4051  {
4052  (*iter)->addLandmark(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
4053  }
4054  else
4055  {
4056  if(uStrNumCmp(_version, "0.7.4") >= 0)
4057  {
4058  links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
4059  }
4060  else // neighbor is 0, loop closures are 1 and 2 (child)
4061  {
4062  links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
4063  }
4064  }
4065  }
4066  else
4067  {
4068  UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
4069  linkType, (*iter)->id(), toId);
4070  }
4071 
4072  rc = sqlite3_step(ppStmt);
4073  }
4074  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4075 
4076  // add links
4077  (*iter)->addLinks(links);
4078 
4079  //reset
4080  rc = sqlite3_reset(ppStmt);
4081  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4082  UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
4083  }
4084 
4085  // Finalize (delete) the statement
4086  rc = sqlite3_finalize(ppStmt);
4087  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4088  }
4089 }
4090 
4091 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
4092 {
4093  UDEBUG("nodes = %d", nodes.size());
4094  if(_ppDb && nodes.size())
4095  {
4096  UTimer timer;
4097  timer.start();
4098  int rc = SQLITE_OK;
4099  sqlite3_stmt * ppStmt = 0;
4100  Signature * s = 0;
4101 
4102  std::string query;
4103  if(uStrNumCmp(_version, "0.8.5") >= 0)
4104  {
4105  if(updateTimestamp)
4106  {
4107  query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
4108  }
4109  else
4110  {
4111  query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
4112  }
4113  }
4114  else
4115  {
4116  if(updateTimestamp)
4117  {
4118  query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
4119  }
4120  else
4121  {
4122  query = "UPDATE Node SET weight=? WHERE id=?;";
4123  }
4124  }
4125  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4126  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4127 
4128  for(std::list<Signature *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
4129  {
4130  s = *i;
4131  int index = 1;
4132  if(s)
4133  {
4134  rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
4135  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4136 
4137  if(uStrNumCmp(_version, "0.8.5") >= 0)
4138  {
4139  if(s->getLabel().empty())
4140  {
4141  rc = sqlite3_bind_null(ppStmt, index++);
4142  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4143  }
4144  else
4145  {
4146  rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
4147  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4148  }
4149  }
4150 
4151  rc = sqlite3_bind_int(ppStmt, index++, s->id());
4152  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4153 
4154  //step
4155  rc=sqlite3_step(ppStmt);
4156  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4157 
4158  rc = sqlite3_reset(ppStmt);
4159  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4160  }
4161  }
4162  // Finalize (delete) the statement
4163  rc = sqlite3_finalize(ppStmt);
4164  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4165 
4166  ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
4167 
4168  // Update links part1
4169  if(uStrNumCmp(_version, "0.18.3") >= 0)
4170  {
4171  query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
4172  }
4173  else
4174  {
4175  query = uFormat("DELETE FROM Link WHERE from_id=?;");
4176  }
4177  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4178  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4179  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4180  {
4181  if((*j)->isLinksModified())
4182  {
4183  rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
4184  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4185 
4186  rc=sqlite3_step(ppStmt);
4187  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4188 
4189  rc = sqlite3_reset(ppStmt);
4190  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4191  }
4192  }
4193  // Finalize (delete) the statement
4194  rc = sqlite3_finalize(ppStmt);
4195  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4196 
4197  // Update links part2
4198  query = queryStepLink();
4199  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4200  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4201  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4202  {
4203  if((*j)->isLinksModified())
4204  {
4205  // Save links
4206  const std::multimap<int, Link> & links = (*j)->getLinks();
4207  for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4208  {
4209  stepLink(ppStmt, i->second);
4210  }
4211  }
4212  }
4213  // Finalize (delete) the statement
4214  rc = sqlite3_finalize(ppStmt);
4215  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4216  ULOGGER_DEBUG("Update Neighbors Time=%fs", timer.ticks());
4217 
4218  // Update word references
4219  query = queryStepWordsChanged();
4220  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4221  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4222  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4223  {
4224  if((*j)->getWordsChanged().size())
4225  {
4226  const std::map<int, int> & wordsChanged = (*j)->getWordsChanged();
4227  for(std::map<int, int>::const_iterator iter=wordsChanged.begin(); iter!=wordsChanged.end(); ++iter)
4228  {
4229  stepWordsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
4230  }
4231  }
4232  }
4233  // Finalize (delete) the statement
4234  rc = sqlite3_finalize(ppStmt);
4235  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4236 
4237  ULOGGER_DEBUG("signatures update=%fs", timer.ticks());
4238  }
4239 }
4240 
4241 void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
4242 {
4243  if(_ppDb && words.size() && updateTimestamp)
4244  {
4245  // Only timestamp update is done here, so don't enter this if at all if false
4246  UTimer timer;
4247  timer.start();
4248  int rc = SQLITE_OK;
4249  sqlite3_stmt * ppStmt = 0;
4250  VisualWord * w = 0;
4251 
4252  std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
4253  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4254  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4255 
4256  for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
4257  {
4258  w = *i;
4259  int index = 1;
4260  UASSERT(w);
4261 
4262  rc = sqlite3_bind_int(ppStmt, index++, w->id());
4263  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4264 
4265  //step
4266  rc=sqlite3_step(ppStmt);
4267  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4268 
4269  rc = sqlite3_reset(ppStmt);
4270  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4271 
4272  }
4273  // Finalize (delete) the statement
4274  rc = sqlite3_finalize(ppStmt);
4275  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4276 
4277  ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
4278  }
4279 }
4280 
4281 void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
4282 {
4283  UDEBUG("");
4284  if(_ppDb && signatures.size())
4285  {
4286  std::string type;
4287  UTimer timer;
4288  timer.start();
4289  int rc = SQLITE_OK;
4290  sqlite3_stmt * ppStmt = 0;
4291 
4292  // Signature table
4293  std::string query = queryStepNode();
4294  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4295  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4296 
4297  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4298  {
4299  _memoryUsedEstimate += (*i)->getMemoryUsed();
4300  // raw data are not kept in database
4301  _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
4302  _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
4303  _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
4304 
4305  stepNode(ppStmt, *i);
4306  }
4307  // Finalize (delete) the statement
4308  rc = sqlite3_finalize(ppStmt);
4309  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4310 
4311  UDEBUG("Time=%fs", timer.ticks());
4312 
4313  // Create new entries in table Link
4314  query = queryStepLink();
4315  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4316  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4317  for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
4318  {
4319  // Save links
4320  const std::multimap<int, Link> & links = (*jter)->getLinks();
4321  for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4322  {
4323  stepLink(ppStmt, i->second);
4324  }
4325  if(uStrNumCmp(_version, "0.18.3") >= 0)
4326  {
4327  // Save landmarks
4328  const std::map<int, Link> & links = (*jter)->getLandmarks();
4329  for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4330  {
4331  stepLink(ppStmt, i->second);
4332  }
4333  }
4334  }
4335  // Finalize (delete) the statement
4336  rc = sqlite3_finalize(ppStmt);
4337  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4338 
4339  UDEBUG("Time=%fs", timer.ticks());
4340 
4341 
4342  // Create new entries in table Feature
4343  query = queryStepKeypoint();
4344  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4345  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4346  float nanFloat = std::numeric_limits<float>::quiet_NaN ();
4347  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4348  {
4349  UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
4350  UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
4351  UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
4352 
4353  for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
4354  {
4355  cv::Point3f pt(nanFloat,nanFloat,nanFloat);
4356  if(!(*i)->getWords3().empty())
4357  {
4358  pt = (*i)->getWords3()[w->second];
4359  }
4360 
4361  cv::Mat descriptor;
4362  if(!(*i)->getWordsDescriptors().empty())
4363  {
4364  descriptor = (*i)->getWordsDescriptors().row(w->second);
4365  }
4366 
4367  stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
4368  }
4369  }
4370  // Finalize (delete) the statement
4371  rc = sqlite3_finalize(ppStmt);
4372  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4373  UDEBUG("Time=%fs", timer.ticks());
4374 
4375  if(uStrNumCmp(_version, "0.20.0") >= 0)
4376  {
4377  // Global descriptor table
4378  std::string query = queryStepGlobalDescriptor();
4379  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4380  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4381 
4382  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4383  {
4384  for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
4385  {
4386  stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
4387  }
4388  }
4389  // Finalize (delete) the statement
4390  rc = sqlite3_finalize(ppStmt);
4391  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4392 
4393  UDEBUG("Time=%fs", timer.ticks());
4394  }
4395 
4396  if(uStrNumCmp(_version, "0.10.0") >= 0)
4397  {
4398  // Add SensorData
4399  query = queryStepSensorData();
4400  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4401  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4402  UDEBUG("Saving %d images", signatures.size());
4403 
4404  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4405  {
4406  if(!(*i)->sensorData().imageCompressed().empty() ||
4407  !(*i)->sensorData().depthOrRightCompressed().empty() ||
4408  !(*i)->sensorData().laserScanCompressed().isEmpty() ||
4409  !(*i)->sensorData().userDataCompressed().empty() ||
4410  !(*i)->sensorData().cameraModels().empty() ||
4411  !(*i)->sensorData().stereoCameraModels().empty())
4412  {
4413  UASSERT((*i)->id() == (*i)->sensorData().id());
4414  stepSensorData(ppStmt, (*i)->sensorData());
4415  }
4416  }
4417 
4418  // Finalize (delete) the statement
4419  rc = sqlite3_finalize(ppStmt);
4420  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4421  UDEBUG("Time=%fs", timer.ticks());
4422  }
4423  else
4424  {
4425  // Add images
4426  query = queryStepImage();
4427  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4428  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4429  UDEBUG("Saving %d images", signatures.size());
4430 
4431  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4432  {
4433  if(!(*i)->sensorData().imageCompressed().empty())
4434  {
4435  stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
4436  }
4437  }
4438 
4439  // Finalize (delete) the statement
4440  rc = sqlite3_finalize(ppStmt);
4441  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4442  UDEBUG("Time=%fs", timer.ticks());
4443 
4444  // Add depths
4445  query = queryStepDepth();
4446  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4447  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4448  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4449  {
4450  //metric
4451  if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().isEmpty())
4452  {
4453  UASSERT((*i)->id() == (*i)->sensorData().id());
4454  stepDepth(ppStmt, (*i)->sensorData());
4455  }
4456  }
4457  // Finalize (delete) the statement
4458  rc = sqlite3_finalize(ppStmt);
4459  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4460  }
4461 
4462  UDEBUG("Time=%fs", timer.ticks());
4463  }
4464 }
4465 
4466 void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
4467 {
4468  UDEBUG("visualWords size=%d", words.size());
4469  if(_ppDb)
4470  {
4471  std::string type;
4472  UTimer timer;
4473  timer.start();
4474  int rc = SQLITE_OK;
4475  sqlite3_stmt * ppStmt = 0;
4476  std::string query;
4477 
4478  // Create new entries in table Map_SS_VW
4479  if(words.size()>0)
4480  {
4481  query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
4482  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4483  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4484  for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
4485  {
4486  const VisualWord * w = *iter;
4487  UASSERT(w);
4488  if(!w->isSaved())
4489  {
4490  rc = sqlite3_bind_int(ppStmt, 1, w->id());
4491  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4492  rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
4493  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4494  UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
4495  if(w->getDescriptor().type() == CV_32F)
4496  {
4497  // CV_32F
4498  rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
4499  }
4500  else
4501  {
4502  // CV_8U
4503  rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
4504  }
4505  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4506 
4507  //execute query
4508  rc=sqlite3_step(ppStmt);
4509  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
4510 
4511  rc = sqlite3_reset(ppStmt);
4512  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4513  }
4514  }
4515  // Finalize (delete) the statement
4516  rc = sqlite3_finalize(ppStmt);
4517  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4518  }
4519 
4520  UDEBUG("Time=%fs", timer.ticks());
4521  }
4522 }
4523 
4524 void DBDriverSqlite3::addLinkQuery(const Link & link) const
4525 {
4526  UDEBUG("");
4527  if(_ppDb)
4528  {
4529  std::string type;
4530  UTimer timer;
4531  timer.start();
4532  int rc = SQLITE_OK;
4533  sqlite3_stmt * ppStmt = 0;
4534 
4535  // Create new entries in table Link
4536  std::string query = queryStepLink();
4537  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4538  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4539 
4540  // Save link
4541  stepLink(ppStmt, link);
4542 
4543  // Finalize (delete) the statement
4544  rc = sqlite3_finalize(ppStmt);
4545  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4546 
4547  UDEBUG("Time=%fs", timer.ticks());
4548  }
4549 
4550 }
4551 
4552 void DBDriverSqlite3::updateLinkQuery(const Link & link) const
4553 {
4554  UDEBUG("");
4555  if(_ppDb)
4556  {
4557  std::string type;
4558  UTimer timer;
4559  timer.start();
4560  int rc = SQLITE_OK;
4561  sqlite3_stmt * ppStmt = 0;
4562 
4563  // Create new entries in table Link
4564  std::string query = queryStepLinkUpdate();
4565  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4566  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4567 
4568  // Save link
4569  stepLink(ppStmt, link);
4570 
4571  // Finalize (delete) the statement
4572  rc = sqlite3_finalize(ppStmt);
4573  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4574 
4575  UDEBUG("Time=%fs", timer.ticks());
4576  }
4577 }
4578 
4579 void DBDriverSqlite3::updateOccupancyGridQuery(
4580  int nodeId,
4581  const cv::Mat & ground,
4582  const cv::Mat & obstacles,
4583  const cv::Mat & empty,
4584  float cellSize,
4585  const cv::Point3f & viewpoint) const
4586 {
4587  UDEBUG("");
4588  if(_ppDb)
4589  {
4590  std::string type;
4591  UTimer timer;
4592  timer.start();
4593  int rc = SQLITE_OK;
4594  sqlite3_stmt * ppStmt = 0;
4595 
4596  // Create query
4597  std::string query = queryStepOccupancyGridUpdate();
4598  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4599  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4600 
4601  // Save occupancy grid
4602  stepOccupancyGridUpdate(ppStmt,
4603  nodeId,
4604  ground,
4605  obstacles,
4606  empty,
4607  cellSize,
4608  viewpoint);
4609 
4610  // Finalize (delete) the statement
4611  rc = sqlite3_finalize(ppStmt);
4612  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4613 
4614  UDEBUG("Time=%fs", timer.ticks());
4615  }
4616 }
4617 
4618 void DBDriverSqlite3::updateDepthImageQuery(
4619  int nodeId,
4620  const cv::Mat & image) const
4621 {
4622  UDEBUG("");
4623  if(_ppDb)
4624  {
4625  std::string type;
4626  UTimer timer;
4627  timer.start();
4628  int rc = SQLITE_OK;
4629  sqlite3_stmt * ppStmt = 0;
4630 
4631  // Create query
4632  std::string query = queryStepDepthUpdate();
4633  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4634  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4635 
4636  // Save depth
4637  stepDepthUpdate(ppStmt,
4638  nodeId,
4639  image);
4640 
4641  // Finalize (delete) the statement
4642  rc = sqlite3_finalize(ppStmt);
4643  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4644 
4645  UDEBUG("Time=%fs", timer.ticks());
4646  }
4647 }
4648 
4649 void DBDriverSqlite3::updateLaserScanQuery(
4650  int nodeId,
4651  const LaserScan & scan) const
4652 {
4653  UDEBUG("");
4654  if(_ppDb)
4655  {
4656  std::string type;
4657  UTimer timer;
4658  timer.start();
4659  int rc = SQLITE_OK;
4660  sqlite3_stmt * ppStmt = 0;
4661 
4662  // Create query
4663  std::string query = queryStepScanUpdate();
4664  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4665  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4666 
4667  // Save depth
4668  stepScanUpdate(ppStmt,
4669  nodeId,
4670  scan);
4671 
4672  // Finalize (delete) the statement
4673  rc = sqlite3_finalize(ppStmt);
4674  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4675 
4676  UDEBUG("Time=%fs", timer.ticks());
4677  }
4678 }
4679 
4680 void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
4681 {
4682  UDEBUG("Ref ID = %d", statistics.refImageId());
4683  if(_ppDb)
4684  {
4685  std::string type;
4686  UTimer timer;
4687  timer.start();
4688  int rc = SQLITE_OK;
4689  sqlite3_stmt * ppStmt = 0;
4690 
4691  // Create query
4692  if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
4693  {
4694  std::string param = Statistics::serializeData(statistics.data());
4695  if(param.size() && statistics.refImageId()>0)
4696  {
4697  std::string query;
4698  if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
4699  {
4700  query = "INSERT INTO Statistics(id, stamp, data, wm_state) values(?,?,?,?);";
4701  }
4702  else
4703  {
4704  query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
4705  }
4706  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4707  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4708 
4709  int index = 1;
4710  rc = sqlite3_bind_int(ppStmt, index++, statistics.refImageId());
4711  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4712  rc = sqlite3_bind_double(ppStmt, index++, statistics.stamp());
4713  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4714 
4715  cv::Mat compressedParam;
4716  if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
4717  {
4718  compressedParam = compressString(param);
4719  rc = sqlite3_bind_blob(ppStmt, index++, compressedParam.data, compressedParam.cols, SQLITE_STATIC);
4720  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4721  }
4722  else
4723  {
4724  rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, SQLITE_STATIC);
4725  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4726  }
4727 
4728  cv::Mat compressedWmState;
4729  if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
4730  {
4731  if(saveWmState && !statistics.wmState().empty())
4732  {
4733  compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
4734  rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
4735  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4736  }
4737  else
4738  {
4739  rc = sqlite3_bind_null(ppStmt, index++);
4740  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4741  }
4742  }
4743 
4744  //step
4745  rc=sqlite3_step(ppStmt);
4746  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4747 
4748  rc = sqlite3_reset(ppStmt);
4749  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4750 
4751  // Finalize (delete) the statement
4752  rc = sqlite3_finalize(ppStmt);
4753  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4754 
4755  UDEBUG("Time=%fs", timer.ticks());
4756  }
4757  }
4758  }
4759 }
4760 
4761 void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
4762 {
4763  UDEBUG("");
4764  if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
4765  {
4766  UTimer timer;
4767  timer.start();
4768  int rc = SQLITE_OK;
4769  sqlite3_stmt * ppStmt = 0;
4770  std::string query;
4771 
4772  // Update table Admin
4773  query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
4774  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4775  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4776 
4777  int index = 1;
4778  cv::Mat compressedImage;
4779  if(image.empty())
4780  {
4781  rc = sqlite3_bind_null(ppStmt, index);
4782  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4783  }
4784  else
4785  {
4786  // compress
4787  if(image.rows == 1 && image.type() == CV_8UC1)
4788  {
4789  // already compressed
4790  compressedImage = image;
4791  }
4792  else
4793  {
4794  compressedImage = compressImage2(image, ".jpg");
4795  }
4796  rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
4797  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4798  }
4799 
4800  //execute query
4801  rc=sqlite3_step(ppStmt);
4802  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4803 
4804  // Finalize (delete) the statement
4805  rc = sqlite3_finalize(ppStmt);
4806  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4807 
4808  UDEBUG("Time=%fs", timer.ticks());
4809  }
4810 }
4811 cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
4812 {
4813  UDEBUG("");
4814  cv::Mat image;
4815  if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
4816  {
4817  UTimer timer;
4818  timer.start();
4819  int rc = SQLITE_OK;
4820  sqlite3_stmt * ppStmt = 0;
4821  std::stringstream query;
4822 
4823  query << "SELECT preview_image "
4824  << "FROM Admin "
4825  << "WHERE version='" << _version.c_str()
4826  <<"';";
4827 
4828  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
4829  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4830 
4831  // Process the result if one
4832  rc = sqlite3_step(ppStmt);
4833  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());
4834  if(rc == SQLITE_ROW)
4835  {
4836  const void * data = 0;
4837  int dataSize = 0;
4838  int index = 0;
4839 
4840  //opt_cloud
4841  data = sqlite3_column_blob(ppStmt, index);
4842  dataSize = sqlite3_column_bytes(ppStmt, index++);
4843  if(dataSize>0 && data)
4844  {
4845  image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
4846  }
4847  UDEBUG("Image=%dx%d", image.cols, image.rows);
4848 
4849  rc = sqlite3_step(ppStmt); // next result...
4850  }
4851  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4852 
4853  // Finalize (delete) the statement
4854  rc = sqlite3_finalize(ppStmt);
4855  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4856  ULOGGER_DEBUG("Time=%fs", timer.ticks());
4857 
4858  }
4859  return image;
4860 }
4861 
4862 void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
4863 {
4864  UDEBUG("poses=%d lastlocalizationPose=%s", (int)poses.size(), lastlocalizationPose.prettyPrint().c_str());
4865  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
4866  {
4867  UTimer timer;
4868  timer.start();
4869  int rc = SQLITE_OK;
4870  sqlite3_stmt * ppStmt = 0;
4871  std::string query;
4872 
4873  // Update table Admin
4874  query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
4875  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4876  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4877 
4878  int index = 1;
4879 
4880  // opt ids and poses
4881  cv::Mat compressedIds;
4882  cv::Mat compressedPoses;
4883  if(poses.empty())
4884  {
4885  rc = sqlite3_bind_null(ppStmt, index++);
4886  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4887  rc = sqlite3_bind_null(ppStmt, index++);
4888  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4889  }
4890  else
4891  {
4892  std::vector<int> serializedIds(poses.size());
4893  std::vector<float> serializedPoses(poses.size()*12);
4894  int i=0;
4895  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4896  {
4897  serializedIds[i] = iter->first;
4898  memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
4899  ++i;
4900  }
4901 
4902  compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
4903  compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
4904 
4905  rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
4906  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4907  rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
4908  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4909  }
4910 
4911  if(lastlocalizationPose.isNull())
4912  {
4913  rc = sqlite3_bind_null(ppStmt, index++);
4914  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4915  }
4916  else
4917  {
4918  UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
4919  rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
4920  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4921  }
4922 
4923  //execute query
4924  rc=sqlite3_step(ppStmt);
4925  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4926 
4927  // Finalize (delete) the statement
4928  rc = sqlite3_finalize(ppStmt);
4929  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4930 
4931  UDEBUG("Time=%fs", timer.ticks());
4932  }
4933 }
4934 
4935 std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
4936 {
4937  UDEBUG("");
4938  std::map<int, Transform> poses;
4939  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
4940  {
4941  UTimer timer;
4942  timer.start();
4943  int rc = SQLITE_OK;
4944  sqlite3_stmt * ppStmt = 0;
4945  std::stringstream query;
4946 
4947  query << "SELECT opt_ids, opt_poses, opt_last_localization "
4948  << "FROM Admin "
4949  << "WHERE version='" << _version.c_str()
4950  <<"';";
4951 
4952  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
4953  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4954 
4955  // Process the result if one
4956  rc = sqlite3_step(ppStmt);
4957  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());
4958  if(rc == SQLITE_ROW)
4959  {
4960  const void * data = 0;
4961  int dataSize = 0;
4962  int index = 0;
4963 
4964  //opt_poses
4965  cv::Mat serializedIds;
4966  data = sqlite3_column_blob(ppStmt, index);
4967  dataSize = sqlite3_column_bytes(ppStmt, index++);
4968  if(dataSize>0 && data)
4969  {
4970  serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
4971  UDEBUG("serializedIds=%d", serializedIds.cols);
4972  }
4973 
4974  data = sqlite3_column_blob(ppStmt, index);
4975  dataSize = sqlite3_column_bytes(ppStmt, index++);
4976  if(dataSize>0 && data)
4977  {
4978  cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
4979  UDEBUG("serializedPoses=%d", serializedPoses.cols);
4980 
4981  UASSERT(serializedIds.cols == serializedPoses.cols/12);
4982  UASSERT(serializedPoses.type() == CV_32FC1);
4983  UASSERT(serializedIds.type() == CV_32SC1);
4984  for(int i=0; i<serializedIds.cols; ++i)
4985  {
4986  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),
4987  serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
4988  serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
4989  poses.insert(std::make_pair(serializedIds.at<int>(i), t));
4990  UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
4991  }
4992  }
4993 
4994  data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
4995  dataSize = sqlite3_column_bytes(ppStmt, index++);
4996  if(lastlocalizationPose)
4997  {
4998  if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
4999  {
5000  memcpy(lastlocalizationPose->data(), data, dataSize);
5001  }
5002  UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
5003  }
5004 
5005  rc = sqlite3_step(ppStmt); // next result...
5006  }
5007  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5008 
5009  // Finalize (delete) the statement
5010  rc = sqlite3_finalize(ppStmt);
5011  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5012  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5013 
5014  }
5015  return poses;
5016 }
5017 
5018 void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
5019 {
5020  UDEBUG("");
5021  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
5022  {
5023  UTimer timer;
5024  timer.start();
5025  int rc = SQLITE_OK;
5026  sqlite3_stmt * ppStmt = 0;
5027  std::string query;
5028 
5029  // Update table Admin
5030  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());
5031  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
5032  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5033 
5034  int index = 1;
5035 
5036  // opt ids and poses
5037  cv::Mat compressedMap;
5038  if(map.empty())
5039  {
5040  rc = sqlite3_bind_null(ppStmt, index++);
5041  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5042  }
5043  else
5044  {
5045  compressedMap = compressData2(map);
5046 
5047  rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
5048  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5049  }
5050 
5051  rc = sqlite3_bind_double(ppStmt, index++, xMin);
5052  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5053  rc = sqlite3_bind_double(ppStmt, index++, yMin);
5054  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5055  rc = sqlite3_bind_double(ppStmt, index++, cellSize);
5056  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5057 
5058  //execute query
5059  rc=sqlite3_step(ppStmt);
5060  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5061 
5062  // Finalize (delete) the statement
5063  rc = sqlite3_finalize(ppStmt);
5064  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5065 
5066  UDEBUG("Time=%fs", timer.ticks());
5067  }
5068 }
5069 
5070 cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
5071 {
5072  UDEBUG("");
5073  cv::Mat map;
5074  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
5075  {
5076  UTimer timer;
5077  timer.start();
5078  int rc = SQLITE_OK;
5079  sqlite3_stmt * ppStmt = 0;
5080  std::stringstream query;
5081 
5082  query << "SELECT opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
5083  << "FROM Admin "
5084  << "WHERE version='" << _version.c_str()
5085  <<"';";
5086 
5087  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
5088  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5089 
5090  // Process the result if one
5091  rc = sqlite3_step(ppStmt);
5092  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());
5093  if(rc == SQLITE_ROW)
5094  {
5095  const void * data = 0;
5096  int dataSize = 0;
5097  int index = 0;
5098 
5099  //opt_map
5100  data = sqlite3_column_blob(ppStmt, index);
5101  dataSize = sqlite3_column_bytes(ppStmt, index++);
5102  if(dataSize>0 && data)
5103  {
5104  map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5105  UDEBUG("map=%d/%d", map.cols, map.rows);
5106  }
5107 
5108  xMin = sqlite3_column_double(ppStmt, index++);
5109  UDEBUG("xMin=%f", xMin);
5110  yMin = sqlite3_column_double(ppStmt, index++);
5111  UDEBUG("yMin=%f", yMin);
5112  cellSize = sqlite3_column_double(ppStmt, index++);
5113  UDEBUG("cellSize=%f", cellSize);
5114 
5115  rc = sqlite3_step(ppStmt); // next result...
5116  }
5117  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5118 
5119  // Finalize (delete) the statement
5120  rc = sqlite3_finalize(ppStmt);
5121  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5122  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5123 
5124  }
5125  return map;
5126 }
5127 
5128 void DBDriverSqlite3::saveOptimizedMeshQuery(
5129  const cv::Mat & cloud,
5130  const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
5131 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5132  const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
5133 #else
5134  const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
5135 #endif
5136  const cv::Mat & textures) const
5137 {
5138  UDEBUG("");
5139  if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
5140  {
5141  UTimer timer;
5142  timer.start();
5143  int rc = SQLITE_OK;
5144  sqlite3_stmt * ppStmt = 0;
5145  std::string query;
5146 
5147  // Update table Admin
5148  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());
5149  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
5150  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5151 
5152  if(cloud.empty())
5153  {
5154  // set all fields to null
5155  for(int i=1; i<=5; ++i)
5156  {
5157  rc = sqlite3_bind_null(ppStmt, i);
5158  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5159  }
5160 
5161  //execute query
5162  rc=sqlite3_step(ppStmt);
5163  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5164  }
5165  else
5166  {
5167  int index = 1;
5168 
5169  // compress and save cloud
5170  cv::Mat compressedCloud;
5171  if(cloud.rows == 1 && cloud.type() == CV_8UC1)
5172  {
5173  // already compressed
5174  compressedCloud = cloud;
5175  }
5176  else
5177  {
5178  UDEBUG("Cloud points=%d", cloud.cols);
5179  compressedCloud = compressData2(cloud);
5180  }
5181  UDEBUG("Cloud compressed bytes=%d", compressedCloud.cols);
5182  rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
5183  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5184 
5185  // opt ids and poses
5186  cv::Mat compressedPolygons;
5187  cv::Mat compressedTexCoords;
5188  cv::Mat compressedTextures;
5189  // polygons
5190  if(polygons.empty())
5191  {
5192  //polygon size
5193  rc = sqlite3_bind_null(ppStmt, index++);
5194  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5195  // polygons
5196  rc = sqlite3_bind_null(ppStmt, index++);
5197  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5198  // tex_coords
5199  rc = sqlite3_bind_null(ppStmt, index++);
5200  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5201  // materials
5202  rc = sqlite3_bind_null(ppStmt, index++);
5203  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5204  }
5205  else
5206  {
5207  std::vector<int> serializedPolygons;
5208  std::vector<float> serializedTexCoords;
5209  int polygonSize = 0;
5210  int totalPolygonIndices = 0;
5211  UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
5212  for(unsigned int t=0; t<polygons.size(); ++t)
5213  {
5214  UDEBUG("t=%d, polygons=%d", t, (int)polygons[t].size());
5215  unsigned int materialPolygonIndices = 0;
5216  for(unsigned int p=0; p<polygons[t].size(); ++p)
5217  {
5218  if(polygonSize == 0)
5219  {
5220  UASSERT(polygons[t][p].size());
5221  polygonSize = polygons[t][p].size();
5222  }
5223  else
5224  {
5225  UASSERT(polygonSize == (int)polygons[t][p].size());
5226  }
5227 
5228  materialPolygonIndices += polygons[t][p].size();
5229  }
5230  totalPolygonIndices += materialPolygonIndices;
5231 
5232  if(!texCoords.empty())
5233  {
5234  UASSERT(materialPolygonIndices == texCoords[t].size());
5235  }
5236  }
5237  UASSERT(totalPolygonIndices>0);
5238  serializedPolygons.resize(totalPolygonIndices+polygons.size());
5239  if(!texCoords.empty())
5240  {
5241  serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
5242  }
5243 
5244  int oi=0;
5245  int ci=0;
5246  for(unsigned int t=0; t<polygons.size(); ++t)
5247  {
5248  serializedPolygons[oi++] = polygons[t].size();
5249  if(!texCoords.empty())
5250  {
5251  serializedTexCoords[ci++] = texCoords[t].size();
5252  }
5253  for(unsigned int p=0; p<polygons[t].size(); ++p)
5254  {
5255  int texIndex = p*polygonSize;
5256  for(unsigned int i=0; i<polygons[t][p].size(); ++i)
5257  {
5258  serializedPolygons[oi++] = polygons[t][p][i];
5259 
5260  if(!texCoords.empty())
5261  {
5262  serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
5263  serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
5264  }
5265  }
5266  }
5267  }
5268 
5269  UDEBUG("serializedPolygons=%d", (int)serializedPolygons.size());
5270  compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
5271 
5272  // polygon size
5273  rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
5274  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5275 
5276  rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
5277  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5278 
5279  // tex coords
5280  if(texCoords.empty())
5281  {
5282  // tex coords
5283  rc = sqlite3_bind_null(ppStmt, index++);
5284  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5285  // materials
5286  rc = sqlite3_bind_null(ppStmt, index++);
5287  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5288  }
5289  else
5290  {
5291  UDEBUG("serializedTexCoords=%d", (int)serializedTexCoords.size());
5292  compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
5293  rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
5294  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5295 
5296  UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
5297  if(textures.rows == 1 && textures.type() == CV_8UC1)
5298  {
5299  //already compressed
5300  compressedTextures = textures;
5301  }
5302  else
5303  {
5304  compressedTextures = compressImage2(textures, ".jpg");
5305  }
5306  rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
5307  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5308  }
5309  }
5310 
5311  //execute query
5312  rc=sqlite3_step(ppStmt);
5313  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5314  }
5315 
5316  // Finalize (delete) the statement
5317  rc = sqlite3_finalize(ppStmt);
5318  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5319 
5320  UDEBUG("Time=%fs", timer.ticks());
5321  }
5322 }
5323 
5324 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
5325  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
5326 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5327  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
5328 #else
5329  std::vector<std::vector<Eigen::Vector2f> > * texCoords,
5330 #endif
5331  cv::Mat * textures) const
5332 {
5333  UDEBUG("");
5334  cv::Mat cloud;
5335  if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
5336  {
5337  UTimer timer;
5338  timer.start();
5339  int rc = SQLITE_OK;
5340  sqlite3_stmt * ppStmt = 0;
5341  std::stringstream query;
5342 
5343  query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
5344  << "FROM Admin "
5345  << "WHERE version='" << _version.c_str()
5346  <<"';";
5347 
5348  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
5349  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5350 
5351  // Process the result if one
5352  rc = sqlite3_step(ppStmt);
5353  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());
5354  if(rc == SQLITE_ROW)
5355  {
5356  const void * data = 0;
5357  int dataSize = 0;
5358  int index = 0;
5359 
5360  //opt_cloud
5361  data = sqlite3_column_blob(ppStmt, index);
5362  dataSize = sqlite3_column_bytes(ppStmt, index++);
5363  if(dataSize>0 && data)
5364  {
5365  cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5366  }
5367  UDEBUG("Cloud=%d points", cloud.cols);
5368 
5369  //opt_polygons_size
5370  int polygonSize = sqlite3_column_int(ppStmt, index++);
5371  UDEBUG("polygonSize=%d", polygonSize);
5372 
5373  //opt_polygons
5374  data = sqlite3_column_blob(ppStmt, index);
5375  dataSize = sqlite3_column_bytes(ppStmt, index++);
5376  if(dataSize>0 && data)
5377  {
5378  UASSERT(polygonSize > 0);
5379  if(polygons)
5380  {
5381  cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5382  UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
5383  UASSERT(serializedPolygons.total());
5384  for(int t=0; t<serializedPolygons.cols; ++t)
5385  {
5386  UASSERT(serializedPolygons.at<int>(t) > 0);
5387  std::vector<std::vector<RTABMAP_PCL_INDEX> > materialPolygons(serializedPolygons.at<int>(t), std::vector<RTABMAP_PCL_INDEX>(polygonSize));
5388  ++t;
5389  UASSERT(t < serializedPolygons.cols);
5390  UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
5391  for(int p=0; p<(int)materialPolygons.size(); ++p)
5392  {
5393  for(int i=0; i<polygonSize; ++i)
5394  {
5395  materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
5396  }
5397  }
5398  t+=materialPolygons.size()*polygonSize;
5399  polygons->push_back(materialPolygons);
5400  }
5401  }
5402 
5403  //opt_tex_coords
5404  data = sqlite3_column_blob(ppStmt, index);
5405  dataSize = sqlite3_column_bytes(ppStmt, index++);
5406  if(dataSize>0 && data)
5407  {
5408  if(texCoords)
5409  {
5410  cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5411  UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
5412  UASSERT(serializedTexCoords.total());
5413  for(int t=0; t<serializedTexCoords.cols; ++t)
5414  {
5415  UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
5416 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5417  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
5418 #else
5419  std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
5420 #endif
5421  ++t;
5422  UASSERT(t < serializedTexCoords.cols);
5423  UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
5424  for(int p=0; p<(int)materialtexCoords.size(); ++p)
5425  {
5426  materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
5427  materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
5428  }
5429  t+=materialtexCoords.size()*2;
5430  texCoords->push_back(materialtexCoords);
5431  }
5432  }
5433 
5434  //opt_tex_materials
5435  data = sqlite3_column_blob(ppStmt, index);
5436  dataSize = sqlite3_column_bytes(ppStmt, index++);
5437  if(dataSize>0 && data)
5438  {
5439  if(textures)
5440  {
5441  *textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5442  UDEBUG("textures=%dx%d", textures->cols, textures->rows);
5443  }
5444  }
5445  }
5446  }
5447 
5448  rc = sqlite3_step(ppStmt); // next result...
5449  }
5450  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5451 
5452  // Finalize (delete) the statement
5453  rc = sqlite3_finalize(ppStmt);
5454  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5455  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5456 
5457  }
5458  return cloud;
5459 }
5460 
5461 std::string DBDriverSqlite3::queryStepNode() const
5462 {
5463  if(uStrNumCmp(_version, "0.18.0") >= 0)
5464  {
5465  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors) VALUES(?,?,?,?,?,?,?,?,?,?);";
5466  }
5467  else if(uStrNumCmp(_version, "0.14.0") >= 0)
5468  {
5469  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps) VALUES(?,?,?,?,?,?,?,?,?);";
5470  }
5471  else if(uStrNumCmp(_version, "0.13.0") >= 0)
5472  {
5473  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
5474  }
5475  else if(uStrNumCmp(_version, "0.11.1") >= 0)
5476  {
5477  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
5478  }
5479  else if(uStrNumCmp(_version, "0.10.1") >= 0)
5480  {
5481  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
5482  }
5483  else if(uStrNumCmp(_version, "0.8.8") >= 0)
5484  {
5485  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
5486  }
5487  else if(uStrNumCmp(_version, "0.8.5") >= 0)
5488  {
5489  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
5490  }
5491  return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
5492 }
5493 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
5494 {
5495  UDEBUG("Save node %d", s->id());
5496  if(!ppStmt || !s)
5497  {
5498  UFATAL("");
5499  }
5500  int rc = SQLITE_OK;
5501 
5502  int index = 1;
5503  rc = sqlite3_bind_int(ppStmt, index++, s->id());
5504  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5505  rc = sqlite3_bind_int(ppStmt, index++, s->mapId());
5506  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5507  rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
5508  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5509  rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
5510  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5511 
5512  if(uStrNumCmp(_version, "0.8.5") >= 0)
5513  {
5514  rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
5515  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5516 
5517  if(s->getLabel().empty())
5518  {
5519  rc = sqlite3_bind_null(ppStmt, index++);
5520  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5521  }
5522  else
5523  {
5524  rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
5525  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5526  }
5527  }
5528 
5529  std::vector<double> gps;
5530  std::vector<double> envSensors;
5531  if(uStrNumCmp(_version, "0.10.1") >= 0)
5532  {
5533  // ignore user_data
5534 
5535  if(uStrNumCmp(_version, "0.11.1") >= 0)
5536  {
5537  rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
5538  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5539 
5540  if(uStrNumCmp(_version, "0.13.0") >= 0)
5541  {
5542  if(s->getVelocity().empty())
5543  {
5544  rc = sqlite3_bind_null(ppStmt, index++);
5545  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5546  }
5547  else
5548  {
5549  rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
5550  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5551  }
5552  }
5553 
5554  if(uStrNumCmp(_version, "0.14.0") >= 0)
5555  {
5556  if(s->sensorData().gps().stamp() <= 0.0)
5557  {
5558  rc = sqlite3_bind_null(ppStmt, index++);
5559  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5560  }
5561  else
5562  {
5563  gps.resize(6,0.0);
5564  gps[0] = s->sensorData().gps().stamp();
5565  gps[1] = s->sensorData().gps().longitude();
5566  gps[2] = s->sensorData().gps().latitude();
5567  gps[3] = s->sensorData().gps().altitude();
5568  gps[4] = s->sensorData().gps().error();
5569  gps[5] = s->sensorData().gps().bearing();
5570  rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
5571  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5572  }
5573 
5574  if(uStrNumCmp(_version, "0.18.0") >= 0)
5575  {
5576  const EnvSensors & sensors = s->sensorData().envSensors();
5577  if(sensors.size() == 0)
5578  {
5579  rc = sqlite3_bind_null(ppStmt, index++);
5580  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5581  }
5582  else
5583  {
5584 
5585  envSensors.resize(sensors.size()*3,0.0);
5586  int j=0;
5587  for(std::map<EnvSensor::Type, EnvSensor>::const_iterator iter=sensors.begin(); iter!=sensors.end(); ++iter, j+=3)
5588  {
5589  envSensors[j] = (double)iter->second.type();
5590  envSensors[j+1] = iter->second.value();
5591  envSensors[j+2] = iter->second.stamp();
5592  }
5593  rc = sqlite3_bind_blob(ppStmt, index++, envSensors.data(), envSensors.size()*sizeof(double), SQLITE_STATIC);
5594  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5595  }
5596  }
5597  }
5598  }
5599  }
5600  else if(uStrNumCmp(_version, "0.8.8") >= 0)
5601  {
5602  if(s->sensorData().userDataCompressed().empty())
5603  {
5604  rc = sqlite3_bind_null(ppStmt, index++);
5605  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5606  }
5607  else
5608  {
5609  rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
5610  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5611  }
5612  }
5613 
5614  //step
5615  rc=sqlite3_step(ppStmt);
5616  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5617 
5618  rc = sqlite3_reset(ppStmt);
5619  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5620 }
5621 
5622 std::string DBDriverSqlite3::queryStepImage() const
5623 {
5624  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5625  return "INSERT INTO Image(id, data) VALUES(?,?);";
5626 }
5627 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
5628  int id,
5629  const cv::Mat & imageBytes) const
5630 {
5631  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5632  UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
5633  if(!ppStmt)
5634  {
5635  UFATAL("");
5636  }
5637 
5638  int rc = SQLITE_OK;
5639  int index = 1;
5640 
5641  rc = sqlite3_bind_int(ppStmt, index++, id);
5642  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5643 
5644  if(!imageBytes.empty())
5645  {
5646  rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
5647  }
5648  else
5649  {
5650  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5651  }
5652  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5653 
5654  //step
5655  rc=sqlite3_step(ppStmt);
5656  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5657 
5658  rc = sqlite3_reset(ppStmt);
5659  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5660 }
5661 
5662 std::string DBDriverSqlite3::queryStepDepth() const
5663 {
5664  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5665  if(uStrNumCmp(_version, "0.8.11") >= 0)
5666  {
5667  return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
5668  }
5669  else if(uStrNumCmp(_version, "0.7.0") >= 0)
5670  {
5671  return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
5672  }
5673  else
5674  {
5675  return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
5676  }
5677 }
5678 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
5679 {
5680  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5681  UDEBUG("Save depth %d (size=%d) depth2d = %d",
5682  sensorData.id(),
5683  (int)sensorData.depthOrRightCompressed().cols,
5684  sensorData.laserScanCompressed().size());
5685  if(!ppStmt)
5686  {
5687  UFATAL("");
5688  }
5689 
5690  int rc = SQLITE_OK;
5691  int index = 1;
5692 
5693  rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
5694  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5695 
5696  if(!sensorData.depthOrRightCompressed().empty())
5697  {
5698  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
5699  }
5700  else
5701  {
5702  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5703  }
5704  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5705 
5706  float fx=0, fyOrBaseline=0, cx=0, cy=0;
5707  Transform localTransform = Transform::getIdentity();
5708  if(sensorData.cameraModels().size())
5709  {
5710  UASSERT_MSG(sensorData.cameraModels().size() == 1,
5711  uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
5712 
5713  fx = sensorData.cameraModels()[0].fx();
5714  fyOrBaseline = sensorData.cameraModels()[0].fy();
5715  cx = sensorData.cameraModels()[0].cx();
5716  cy = sensorData.cameraModels()[0].cy();
5717  localTransform = sensorData.cameraModels()[0].localTransform();
5718  }
5719  else if(sensorData.stereoCameraModels().size())
5720  {
5721  UASSERT_MSG(sensorData.stereoCameraModels().size() == 1,
5722  uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
5723  fx = sensorData.stereoCameraModels()[0].left().fx();
5724  fyOrBaseline = sensorData.stereoCameraModels()[0].baseline();
5725  cx = sensorData.stereoCameraModels()[0].left().cx();
5726  cy = sensorData.stereoCameraModels()[0].left().cy();
5727  localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
5728  }
5729 
5730  if(uStrNumCmp(_version, "0.7.0") >= 0)
5731  {
5732  rc = sqlite3_bind_double(ppStmt, index++, fx);
5733  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5734  rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
5735  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5736  rc = sqlite3_bind_double(ppStmt, index++, cx);
5737  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5738  rc = sqlite3_bind_double(ppStmt, index++, cy);
5739  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5740  }
5741  else
5742  {
5743  rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
5744  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5745  }
5746 
5747  rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
5748  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5749 
5750  if(!sensorData.laserScanCompressed().isEmpty())
5751  {
5752  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, (int)sensorData.laserScanCompressed().size(), SQLITE_STATIC);
5753  }
5754  else
5755  {
5756  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5757  }
5758  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5759 
5760  if(uStrNumCmp(_version, "0.8.11") >= 0)
5761  {
5762  rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
5763  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5764  }
5765 
5766  //step
5767  rc=sqlite3_step(ppStmt);
5768  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5769 
5770  rc = sqlite3_reset(ppStmt);
5771  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5772 }
5773 
5774 std::string DBDriverSqlite3::queryStepDepthUpdate() const
5775 {
5776  if(uStrNumCmp(_version, "0.10.0") < 0)
5777  {
5778  return "UPDATE Depth SET data=? WHERE id=?;";
5779  }
5780  else
5781  {
5782  return "UPDATE Data SET depth=? WHERE id=?;";
5783  }
5784 }
5785 void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
5786 {
5787  if(!ppStmt)
5788  {
5789  UFATAL("");
5790  }
5791 
5792  int rc = SQLITE_OK;
5793  int index = 1;
5794 
5795  cv::Mat imageCompressed;
5796  if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
5797  {
5798  // compress
5799  imageCompressed = compressImage2(image, ".png");
5800  }
5801  else
5802  {
5803  imageCompressed = image;
5804  }
5805  if(!imageCompressed.empty())
5806  {
5807  rc = sqlite3_bind_blob(ppStmt, index++, imageCompressed.data, (int)imageCompressed.cols, SQLITE_STATIC);
5808  }
5809  else
5810  {
5811  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5812  }
5813  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5814 
5815  //id
5816  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
5817  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5818 
5819  //step
5820  rc=sqlite3_step(ppStmt);
5821  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5822 
5823  rc = sqlite3_reset(ppStmt);
5824  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5825 }
5826 
5827 std::string DBDriverSqlite3::queryStepScanUpdate() const
5828 {
5829  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
5830  if(uStrNumCmp(_version, "0.11.10") >= 0)
5831  {
5832  return "UPDATE Data SET scan_info=?, scan=? WHERE id=?;";
5833  }
5834  else if(uStrNumCmp(_version, "0.10.7") >= 0)
5835  {
5836  return "UPDATE Data SET scan_max_pts=?, scan_max_range=?, scan=? WHERE id=?;";
5837  }
5838  else
5839  {
5840  return "UPDATE Data SET scan_max_pts=? scan=? WHERE id=?;";
5841  }
5842 }
5843 void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & scan) const
5844 {
5845  if(!ppStmt)
5846  {
5847  UFATAL("");
5848  }
5849 
5850  int rc = SQLITE_OK;
5851  int index = 1;
5852 
5853  std::vector<float> scanInfo;
5854  if(uStrNumCmp(_version, "0.11.10") >= 0)
5855  {
5856  if(scan.maxPoints() > 0 ||
5857  scan.rangeMax() > 0 ||
5858  (uStrNumCmp(_version, "0.16.1")>=0 && scan.format() != LaserScan::kUnknown) ||
5859  (!scan.localTransform().isNull() && !scan.localTransform().isIdentity()))
5860  {
5861  if(uStrNumCmp(_version, "0.16.1") >=0)
5862  {
5863  if(uStrNumCmp(_version, "0.18.0") >=0)
5864  {
5865  scanInfo.resize(7 + Transform().size());
5866  scanInfo[0] = scan.format();
5867  scanInfo[1] = scan.rangeMin();
5868  scanInfo[2] = scan.rangeMax();
5869  scanInfo[3] = scan.angleMin();
5870  scanInfo[4] = scan.angleMax();
5871  scanInfo[5] = scan.angleIncrement();
5872  scanInfo[6] = scan.maxPoints(); // only for backward compatibility
5873  const Transform & localTransform = scan.localTransform();
5874  memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
5875  }
5876  else
5877  {
5878  scanInfo.resize(3 + Transform().size());
5879  scanInfo[0] = scan.maxPoints();
5880  scanInfo[1] = scan.rangeMax();
5881  scanInfo[2] = scan.format();
5882  const Transform & localTransform = scan.localTransform();
5883  memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
5884  }
5885  }
5886  else
5887  {
5888  scanInfo.resize(2 + Transform().size());
5889  scanInfo[0] = scan.maxPoints();
5890  scanInfo[1] = scan.rangeMax();
5891  const Transform & localTransform = scan.localTransform();
5892  memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
5893  }
5894  }
5895 
5896  if(scanInfo.size())
5897  {
5898  rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
5899  }
5900  else
5901  {
5902  rc = sqlite3_bind_null(ppStmt, index++);
5903  }
5904  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5905 
5906  }
5907  else
5908  {
5909  // scan_max_pts
5910  rc = sqlite3_bind_int(ppStmt, index++, scan.maxPoints());
5911  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5912 
5913  // scan_max_range
5914  if(uStrNumCmp(_version, "0.10.7") >= 0)
5915  {
5916  rc = sqlite3_bind_double(ppStmt, index++, scan.rangeMax());
5917  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5918  }
5919  }
5920 
5921  // scan
5922  cv::Mat scanCompressed;
5923  if(scan.isCompressed())
5924  {
5925  scanCompressed = scan.data();
5926  }
5927  else
5928  {
5929  scanCompressed = compressData2(scan.data());
5930  }
5931  if(!scanCompressed.empty())
5932  {
5933  rc = sqlite3_bind_blob(ppStmt, index++, scanCompressed.data, scanCompressed.total(), SQLITE_STATIC);
5934  }
5935  else
5936  {
5937  rc = sqlite3_bind_null(ppStmt, index++);
5938  }
5939  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5940 
5941  //id
5942  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
5943  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5944 
5945  //step
5946  rc=sqlite3_step(ppStmt);
5947  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5948 
5949  rc = sqlite3_reset(ppStmt);
5950  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5951 }
5952 
5953 std::string DBDriverSqlite3::queryStepSensorData() const
5954 {
5955  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
5956  if(uStrNumCmp(_version, "0.16.0") >= 0)
5957  {
5958  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(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
5959  }
5960  else if(uStrNumCmp(_version, "0.11.10") >= 0)
5961  {
5962  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
5963  }
5964  else if(uStrNumCmp(_version, "0.10.7") >= 0)
5965  {
5966  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
5967  }
5968  else if(uStrNumCmp(_version, "0.10.1") >= 0)
5969  {
5970  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
5971  }
5972  else
5973  {
5974  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
5975  }
5976 }
5977 void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
5978  const SensorData & sensorData) const
5979 {
5980  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
5981  UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
5982  sensorData.id(),
5983  (int)sensorData.imageCompressed().cols,
5984  (int)sensorData.depthOrRightCompressed().cols,
5985  sensorData.laserScanCompressed().size());
5986  if(!ppStmt)
5987  {
5988  UFATAL("");
5989  }
5990 
5991  int rc = SQLITE_OK;
5992  int index = 1;
5993 
5994  // id
5995  rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
5996  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5997 
5998  // image
5999  if(!sensorData.imageCompressed().empty())
6000  {
6001  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
6002  }
6003  else
6004  {
6005  rc = sqlite3_bind_null(ppStmt, index++);
6006  }
6007  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6008 
6009  // depth or right image
6010  if(!sensorData.depthOrRightCompressed().empty())
6011  {
6012  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
6013  }
6014  else
6015  {
6016  rc = sqlite3_bind_null(ppStmt, index++);
6017  }
6018  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6019 
6020  // calibration
6021  std::vector<unsigned char> calibrationData;
6022  std::vector<float> calibration;
6023  // multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
6024  // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
6025  if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
6026  {
6027  if(uStrNumCmp(_version, "0.18.0") >= 0)
6028  {
6029  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6030  {
6031  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6032  std::vector<unsigned char> data = sensorData.cameraModels()[i].serialize();
6033  UASSERT(!data.empty());
6034  unsigned int oldSize = calibrationData.size();
6035  calibrationData.resize(calibrationData.size() + data.size());
6036  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
6037  }
6038  }
6039  else if(uStrNumCmp(_version, "0.11.2") >= 0)
6040  {
6041  calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
6042  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6043  {
6044  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6045  const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
6046  calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
6047  calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
6048  calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
6049  calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
6050  calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
6051  calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
6052  memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
6053  }
6054  }
6055  else
6056  {
6057  calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
6058  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6059  {
6060  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6061  const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
6062  calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
6063  calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
6064  calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
6065  calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
6066  memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
6067  }
6068  }
6069  }
6070  else if(sensorData.stereoCameraModels().size() && sensorData.stereoCameraModels()[0].isValidForProjection())
6071  {
6072  if(uStrNumCmp(_version, "0.18.0") >= 0)
6073  {
6074  for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
6075  {
6076  UASSERT(sensorData.stereoCameraModels()[i].isValidForProjection());
6077  std::vector<unsigned char> data = sensorData.stereoCameraModels()[i].serialize();
6078  UASSERT(!data.empty());
6079  unsigned int oldSize = calibrationData.size();
6080  calibrationData.resize(calibrationData.size() + data.size());
6081  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
6082  }
6083  }
6084  else
6085  {
6086  UASSERT_MSG(sensorData.stereoCameraModels().size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
6087  const Transform & localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
6088  calibration.resize(7+localTransform.size());
6089  calibration[0] = sensorData.stereoCameraModels()[0].left().fx();
6090  calibration[1] = sensorData.stereoCameraModels()[0].left().fy();
6091  calibration[2] = sensorData.stereoCameraModels()[0].left().cx();
6092  calibration[3] = sensorData.stereoCameraModels()[0].left().cy();
6093  calibration[4] = sensorData.stereoCameraModels()[0].baseline();
6094  calibration[5] = sensorData.stereoCameraModels()[0].left().imageWidth();
6095  calibration[6] = sensorData.stereoCameraModels()[0].left().imageHeight();
6096  memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
6097  }
6098  }
6099 
6100  if(calibrationData.size())
6101  {
6102  rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
6103  }
6104  else if(calibration.size())
6105  {
6106  rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
6107  }
6108  else
6109  {
6110  rc = sqlite3_bind_null(ppStmt, index++);
6111  }
6112  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6113 
6114  std::vector<float> scanInfo;
6115  if(uStrNumCmp(_version, "0.11.10") >= 0)
6116  {
6117  if(sensorData.laserScanCompressed().maxPoints() > 0 ||
6118  sensorData.laserScanCompressed().rangeMax() > 0 ||
6119  (uStrNumCmp(_version, "0.16.1")>=0 && sensorData.laserScanCompressed().format() != LaserScan::kUnknown) ||
6120  (!sensorData.laserScanCompressed().localTransform().isNull() && !sensorData.laserScanCompressed().localTransform().isIdentity()))
6121  {
6122  if(uStrNumCmp(_version, "0.16.1") >=0)
6123  {
6124  if(uStrNumCmp(_version, "0.18.0") >=0)
6125  {
6126  scanInfo.resize(7 + Transform().size());
6127  scanInfo[0] = sensorData.laserScanCompressed().format();
6128  scanInfo[1] = sensorData.laserScanCompressed().rangeMin();
6129  scanInfo[2] = sensorData.laserScanCompressed().rangeMax();
6130  scanInfo[3] = sensorData.laserScanCompressed().angleMin();
6131  scanInfo[4] = sensorData.laserScanCompressed().angleMax();
6132  scanInfo[5] = sensorData.laserScanCompressed().angleIncrement();
6133  scanInfo[6] = sensorData.laserScanCompressed().maxPoints(); // only for backward compatibility
6134  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6135  memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
6136  }
6137  else
6138  {
6139  scanInfo.resize(3 + Transform().size());
6140  scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
6141  scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
6142  scanInfo[2] = sensorData.laserScanCompressed().format();
6143  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6144  memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
6145  }
6146  }
6147  else
6148  {
6149  scanInfo.resize(2 + Transform().size());
6150  scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
6151  scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
6152  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6153  memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
6154  }
6155  }
6156 
6157  if(scanInfo.size())
6158  {
6159  rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
6160  }
6161  else
6162  {
6163  rc = sqlite3_bind_null(ppStmt, index++);
6164  }
6165  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6166 
6167  }
6168  else
6169  {
6170  // scan_max_pts
6171  rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
6172  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6173 
6174  // scan_max_range
6175  if(uStrNumCmp(_version, "0.10.7") >= 0)
6176  {
6177  rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanCompressed().rangeMax());
6178  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6179  }
6180  }
6181 
6182  // scan
6183  if(!sensorData.laserScanCompressed().isEmpty())
6184  {
6185  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, sensorData.laserScanCompressed().size(), SQLITE_STATIC);
6186  }
6187  else
6188  {
6189  rc = sqlite3_bind_null(ppStmt, index++);
6190  }
6191  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6192 
6193  if(uStrNumCmp(_version, "0.10.1") >= 0)
6194  {
6195  // user_data
6196  if(!sensorData.userDataCompressed().empty())
6197  {
6198  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
6199  }
6200  else
6201  {
6202  rc = sqlite3_bind_null(ppStmt, index++);
6203  }
6204  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6205  }
6206 
6207  if(uStrNumCmp(_version, "0.11.10") >= 0)
6208  {
6209  //ground_cells
6210  if(sensorData.gridGroundCellsCompressed().empty())
6211  {
6212  rc = sqlite3_bind_null(ppStmt, index++);
6213  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6214  }
6215  else
6216  {
6217  // compress
6218  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
6219  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6220  }
6221 
6222  //obstacle_cells
6223  if(sensorData.gridObstacleCellsCompressed().empty())
6224  {
6225  rc = sqlite3_bind_null(ppStmt, index++);
6226  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6227  }
6228  else
6229  {
6230  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
6231  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6232  }
6233 
6234  if(uStrNumCmp(_version, "0.16.0") >= 0)
6235  {
6236  //empty_cells
6237  if(sensorData.gridEmptyCellsCompressed().empty())
6238  {
6239  rc = sqlite3_bind_null(ppStmt, index++);
6240  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6241  }
6242  else
6243  {
6244  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
6245  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6246  }
6247  }
6248 
6249  //cell_size
6250  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
6251  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6252 
6253  //view_point
6254  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
6255  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6256  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
6257  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6258  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
6259  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6260  }
6261 
6262  //step
6263  rc=sqlite3_step(ppStmt);
6264  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6265 
6266  rc = sqlite3_reset(ppStmt);
6267  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6268 }
6269 
6270 std::string DBDriverSqlite3::queryStepLinkUpdate() const
6271 {
6272  if(uStrNumCmp(_version, "0.13.0") >= 0)
6273  {
6274  return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
6275  }
6276  else if(uStrNumCmp(_version, "0.10.10") >= 0)
6277  {
6278  return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
6279  }
6280  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6281  {
6282  return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
6283  }
6284  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6285  {
6286  return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
6287  }
6288  else
6289  {
6290  return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
6291  }
6292 }
6293 std::string DBDriverSqlite3::queryStepLink() const
6294 {
6295  // from_id, to_id are at the end to match the update query above
6296  if(uStrNumCmp(_version, "0.13.0") >= 0)
6297  {
6298  return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
6299  }
6300  else if(uStrNumCmp(_version, "0.10.10") >= 0)
6301  {
6302  return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
6303  }
6304  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6305  {
6306  return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
6307  }
6308  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6309  {
6310  return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
6311  }
6312  else
6313  {
6314  return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
6315  }
6316 }
6317 void DBDriverSqlite3::stepLink(
6318  sqlite3_stmt * ppStmt,
6319  const Link & link) const
6320 {
6321  if(!ppStmt)
6322  {
6323  UFATAL("");
6324  }
6325  UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
6326 
6327  // Don't save virtual links
6328  if(link.type()==Link::kVirtualClosure)
6329  {
6330  UDEBUG("Virtual link ignored....");
6331  return;
6332  }
6333 
6334  int rc = SQLITE_OK;
6335  int index = 1;
6336  rc = sqlite3_bind_int(ppStmt, index++, link.type());
6337  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6338 
6339  if(uStrNumCmp(_version, "0.13.0") >= 0)
6340  {
6341  // information_matrix
6342  rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
6343  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6344  }
6345  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6346  {
6347  rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
6348  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6349  rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
6350  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6351  }
6352  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6353  {
6354  rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
6355  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6356  }
6357 
6358  rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
6359  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6360 
6361  if(uStrNumCmp(_version, "0.10.10") >= 0)
6362  {
6363  // user_data
6364  if(!link.userDataCompressed().empty())
6365  {
6366  rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
6367  }
6368  else
6369  {
6370  rc = sqlite3_bind_null(ppStmt, index++);
6371  }
6372  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6373  }
6374 
6375  rc = sqlite3_bind_int(ppStmt, index++, link.from());
6376  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6377  rc = sqlite3_bind_int(ppStmt, index++, link.to());
6378  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6379 
6380  rc=sqlite3_step(ppStmt);
6381  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6382 
6383  rc=sqlite3_reset(ppStmt);
6384  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6385 }
6386 
6387 std::string DBDriverSqlite3::queryStepWordsChanged() const
6388 {
6389  if(uStrNumCmp(_version, "0.13.0") >= 0)
6390  {
6391  return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
6392  }
6393  else
6394  {
6395  return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
6396  }
6397 }
6398 void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
6399 {
6400  if(!ppStmt)
6401  {
6402  UFATAL("");
6403  }
6404  int rc = SQLITE_OK;
6405  int index = 1;
6406  rc = sqlite3_bind_int(ppStmt, index++, newWordId);
6407  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6408  rc = sqlite3_bind_int(ppStmt, index++, oldWordId);
6409  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6410  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6411  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6412 
6413  rc=sqlite3_step(ppStmt);
6414  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6415 
6416  rc=sqlite3_reset(ppStmt);
6417  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6418 }
6419 
6420 std::string DBDriverSqlite3::queryStepKeypoint() const
6421 {
6422  if(uStrNumCmp(_version, "0.13.0") >= 0)
6423  {
6424  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
6425  }
6426  else if(uStrNumCmp(_version, "0.12.0") >= 0)
6427  {
6428  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
6429  }
6430  else if(uStrNumCmp(_version, "0.11.2") >= 0)
6431  {
6432  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(?,?,?,?,?,?,?,?,?,?,?,?);";
6433  }
6434  return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
6435 }
6436 void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
6437  int nodeId,
6438  int wordId,
6439  const cv::KeyPoint & kp,
6440  const cv::Point3f & pt,
6441  const cv::Mat & descriptor) const
6442 {
6443  if(!ppStmt)
6444  {
6445  UFATAL("");
6446  }
6447  int rc = SQLITE_OK;
6448  int index = 1;
6449  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6450  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6451  rc = sqlite3_bind_int(ppStmt, index++, wordId);
6452  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6453  rc = sqlite3_bind_double(ppStmt, index++, kp.pt.x);
6454  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6455  rc = sqlite3_bind_double(ppStmt, index++, kp.pt.y);
6456  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6457  rc = sqlite3_bind_int(ppStmt, index++, kp.size);
6458  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6459  rc = sqlite3_bind_double(ppStmt, index++, kp.angle);
6460  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6461  rc = sqlite3_bind_double(ppStmt, index++, kp.response);
6462  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6463  if(uStrNumCmp(_version, "0.12.0") >= 0)
6464  {
6465  rc = sqlite3_bind_int(ppStmt, index++, kp.octave);
6466  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6467  }
6468 
6469  if(uIsFinite(pt.x))
6470  {
6471  rc = sqlite3_bind_double(ppStmt, index++, pt.x);
6472  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6473  }
6474  else
6475  {
6476  rc = sqlite3_bind_null(ppStmt, index++);
6477  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6478  }
6479 
6480  if(uIsFinite(pt.y))
6481  {
6482  rc = sqlite3_bind_double(ppStmt, index++, pt.y);
6483  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6484  }
6485  else
6486  {
6487  rc = sqlite3_bind_null(ppStmt, index++);
6488  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6489  }
6490 
6491  if(uIsFinite(pt.z))
6492  {
6493  rc = sqlite3_bind_double(ppStmt, index++, pt.z);
6494  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6495  }
6496  else
6497  {
6498  rc = sqlite3_bind_null(ppStmt, index++);
6499  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6500  }
6501 
6502  //descriptor
6503  if(uStrNumCmp(_version, "0.11.2") >= 0)
6504  {
6505  rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
6506  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6507  UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
6508  if(descriptor.empty())
6509  {
6510  rc = sqlite3_bind_null(ppStmt, index++);
6511  }
6512  else
6513  {
6514  if(descriptor.type() == CV_32F)
6515  {
6516  // CV_32F
6517  rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
6518  }
6519  else
6520  {
6521  // CV_8U
6522  rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
6523  }
6524  }
6525  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6526  }
6527 
6528  rc=sqlite3_step(ppStmt);
6529  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6530 
6531  rc = sqlite3_reset(ppStmt);
6532  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6533 }
6534 
6535 std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
6536 {
6537  UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
6538  return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
6539 }
6540 void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
6541  int nodeId,
6542  const GlobalDescriptor & descriptor) const
6543 {
6544  if(!ppStmt)
6545  {
6546  UFATAL("");
6547  }
6548  int rc = SQLITE_OK;
6549  int index = 1;
6550 
6551  //node_if
6552  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6553  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6554 
6555  //type
6556  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6557  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6558 
6559  //info
6560  std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
6561  if(infoBytes.empty())
6562  {
6563  rc = sqlite3_bind_null(ppStmt, index++);
6564  }
6565  else
6566  {
6567  rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
6568  }
6569  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6570 
6571  //data
6572  std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
6573  if(infoBytes.empty())
6574  {
6575  rc = sqlite3_bind_null(ppStmt, index++);
6576  }
6577  else
6578  {
6579  rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
6580  }
6581  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6582 
6583  rc=sqlite3_step(ppStmt);
6584  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6585 
6586  rc = sqlite3_reset(ppStmt);
6587  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6588 }
6589 
6590 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
6591 {
6592  UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
6593  if(uStrNumCmp(_version, "0.16.0") >= 0)
6594  {
6595  return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
6596  }
6597  return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
6598 }
6599 void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
6600  int nodeId,
6601  const cv::Mat & ground,
6602  const cv::Mat & obstacles,
6603  const cv::Mat & empty,
6604  float cellSize,
6605  const cv::Point3f & viewpoint) const
6606 {
6607  UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
6608  UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
6609  UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
6610  UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
6611  UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
6612  nodeId,
6613  ground.cols,
6614  obstacles.cols,
6615  empty.cols,
6616  cellSize,
6617  viewpoint.x,
6618  viewpoint.y,
6619  viewpoint.z);
6620  if(!ppStmt)
6621  {
6622  UFATAL("");
6623  }
6624 
6625  int rc = SQLITE_OK;
6626  int index = 1;
6627 
6628  //ground_cells
6629  if(ground.empty())
6630  {
6631  rc = sqlite3_bind_null(ppStmt, index++);
6632  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6633  }
6634  else
6635  {
6636  // compress
6637  rc = sqlite3_bind_blob(ppStmt, index++, ground.data, ground.cols, SQLITE_STATIC);
6638  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6639  }
6640 
6641  //obstacle_cells
6642  if(obstacles.empty())
6643  {
6644  rc = sqlite3_bind_null(ppStmt, index++);
6645  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6646  }
6647  else
6648  {
6649  rc = sqlite3_bind_blob(ppStmt, index++, obstacles.data, obstacles.cols, SQLITE_STATIC);
6650  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6651  }
6652 
6653  if(uStrNumCmp(_version, "0.16.0") >= 0)
6654  {
6655  //empty_cells
6656  if(empty.empty())
6657  {
6658  rc = sqlite3_bind_null(ppStmt, index++);
6659  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6660  }
6661  else
6662  {
6663  rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
6664  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6665  }
6666  }
6667 
6668  //cell_size
6669  rc = sqlite3_bind_double(ppStmt, index++, cellSize);
6670  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6671 
6672  //view_point
6673  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.x);
6674  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6675  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.y);
6676  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6677  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.z);
6678  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6679 
6680  // id
6681  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6682  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6683 
6684 
6685  //step
6686  rc=sqlite3_step(ppStmt);
6687  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6688 
6689  rc = sqlite3_reset(ppStmt);
6690  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6691 }
6692 
6693 } // namespace rtabmap
sqlite3_memory_used
#define sqlite3_memory_used
Definition: sqlite3ext.h:406
d
d
rtabmap::DBDriverSqlite3::addLinkQuery
virtual void addLinkQuery(const Link &link) const
Definition: DBDriverSqlite3.cpp:4524
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::DBDriverSqlite3::setTempStore
void setTempStore(int tempStore)
Definition: DBDriverSqlite3.cpp:170
rtabmap::Statistics::wmState
const std::vector< int > & wmState() const
Definition: Statistics.h:291
rtabmap::DBDriver::getTargetVersion
const std::string & getTargetVersion() const
Definition: DBDriver.h:73
sqlite3_step
#define sqlite3_step
Definition: sqlite3ext.h:370
rtabmap::GPS::error
const double & error() const
Definition: GPS.h:63
rtabmap::uncompressData
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat &bytes)
Definition: Compression.cpp:231
rtabmap::SensorData::gridObstacleCellsCompressed
const cv::Mat & gridObstacleCellsCompressed() const
Definition: SensorData.h:263
UTimer
Definition: UTimer.h:46
rtabmap::DBDriverSqlite3::getLastParametersQuery
virtual ParametersMap getLastParametersQuery() const
Definition: DBDriverSqlite3.cpp:1066
rtabmap::DBDriverSqlite3::getTotalNodesSizeQuery
virtual int getTotalNodesSizeQuery() const
Definition: DBDriverSqlite3.cpp:1017
rtabmap::DBDriverSqlite3::queryStepSensorData
std::string queryStepSensorData() const
Definition: DBDriverSqlite3.cpp:5953
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
UFile::length
long length()
Definition: UFile.h:110
sqlite3_errcode
#define sqlite3_errcode
Definition: sqlite3ext.h:322
rtabmap::Signature::getGroundTruthPose
const Transform & getGroundTruthPose() const
Definition: Signature.h:134
rtabmap::DBDriverSqlite3::stepKeypoint
void stepKeypoint(sqlite3_stmt *ppStmt, int nodeID, int wordId, const cv::KeyPoint &kp, const cv::Point3f &pt, const cv::Mat &descriptor) const
Definition: DBDriverSqlite3.cpp:6436
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
rtabmap::DBDriverSqlite3::getDepthImagesMemoryUsedQuery
virtual long getDepthImagesMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:662
rtabmap::Signature::getVelocity
const std::vector< float > & getVelocity() const
Definition: Signature.h:135
rtabmap::DBDriverSqlite3::loadOptimizedPosesQuery
virtual std::map< int, Transform > loadOptimizedPosesQuery(Transform *lastlocalizationPose=0) const
Definition: DBDriverSqlite3.cpp:4935
f
f
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:119
rtabmap::DBDriverSqlite3::getAllNodeIdsQuery
virtual void getAllNodeIdsQuery(std::set< int > &ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const
Definition: DBDriverSqlite3.cpp:2416
rtabmap::DBDriverSqlite3::loadLinksQuery
virtual void loadLinksQuery(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriverSqlite3.cpp:3768
sqlite3_open
#define sqlite3_open
Definition: sqlite3ext.h:344
ULOGGER_INFO
#define ULOGGER_INFO(...)
Definition: ULogger.h:54
sqlite3_threadsafe
#define sqlite3_threadsafe
Definition: sqlite3ext.h:421
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:719
rtabmap::SensorData::gridGroundCellsCompressed
const cv::Mat & gridGroundCellsCompressed() const
Definition: SensorData.h:261
rtabmap::DBDriverSqlite3::loadOrSaveDb
int loadOrSaveDb(sqlite3 *pInMemory, const std::string &fileName, int isSave) const
Definition: DBDriverSqlite3.cpp:236
sqlite3_backup_finish
#define sqlite3_backup_finish
Definition: sqlite3ext.h:432
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:170
rtabmap::DBDriverSqlite3::addStatisticsQuery
virtual void addStatisticsQuery(const Statistics &statistics, bool saveWmState) const
Definition: DBDriverSqlite3.cpp:4680
sqlite3_backup
Definition: sqlite3.c:58906
rtabmap::DBDriverSqlite3::setCacheSize
void setCacheSize(unsigned int cacheSize)
Definition: DBDriverSqlite3.cpp:97
sqlite3_backup_init
#define sqlite3_backup_init
Definition: sqlite3ext.h:433
rtabmap::DBDriverSqlite3::getFeaturesMemoryUsedQuery
virtual long getFeaturesMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:873
sqlite3_errmsg
#define sqlite3_errmsg
Definition: sqlite3ext.h:323
data
data
rtabmap::StereoCameraModel::deserialize
unsigned int deserialize(const std::vector< unsigned char > &data)
Definition: StereoCameraModel.cpp:464
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
rtabmap::DBDriverSqlite3::getUserDataMemoryUsedQuery
virtual long getUserDataMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:813
rtabmap::Signature::mapId
int mapId() const
Definition: Signature.h:71
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
sqlite3_stmt
struct sqlite3_stmt sqlite3_stmt
Definition: DBDriverSqlite3.h:35
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
SQLITE_ROW
#define SQLITE_ROW
Definition: sqlite3.c:458
rtabmap::DBDriverSqlite3::getStatisticsQuery
virtual std::map< std::string, float > getStatisticsQuery(int nodeId, double &stamp, std::vector< int > *wmState) const
Definition: DBDriverSqlite3.cpp:1112
rtabmap::DBDriverSqlite3::savePreviewImageQuery
virtual void savePreviewImageQuery(const cv::Mat &image) const
Definition: DBDriverSqlite3.cpp:4761
rtabmap::DBDriverSqlite3::getTotalDictionarySizeQuery
virtual int getTotalDictionarySizeQuery() const
Definition: DBDriverSqlite3.cpp:1041
rtabmap::GlobalDescriptor::info
const cv::Mat info() const
Definition: GlobalDescriptor.h:50
SQLITE_DONE
#define SQLITE_DONE
Definition: sqlite3.c:459
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::Signature::getLabel
const std::string & getLabel() const
Definition: Signature.h:77
rtabmap::DBDriverSqlite3::loadWordsQuery
virtual void loadWordsQuery(const std::set< int > &wordIds, std::list< VisualWord *> &vws) const
Definition: DBDriverSqlite3.cpp:3673
rtabmap::DBDriverSqlite3::saveOptimizedMeshQuery
virtual void saveOptimizedMeshQuery(const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, const cv::Mat &textures) const
Definition: DBDriverSqlite3.cpp:5128
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
sqlite3_column_int64
#define sqlite3_column_int64
Definition: sqlite3ext.h:298
sqlite3_exec
#define sqlite3_exec
Definition: sqlite3ext.h:325
rtabmap::DBDriverSqlite3::getWordsMemoryUsedQuery
virtual long getWordsMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:849
rtabmap::DBDriverSqlite3::getNodesMemoryUsedQuery
virtual long getNodesMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:538
UFATAL
#define UFATAL(...)
rtabmap::DBDriverSqlite3::getInvertedIndexNiQuery
virtual void getInvertedIndexNiQuery(int signatureId, int &ni) const
Definition: DBDriverSqlite3.cpp:2661
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
rtabmap::DBDriverSqlite3::setJournalMode
void setJournalMode(int journalMode)
Definition: DBDriverSqlite3.cpp:108
rtabmap::SensorData::gridCellSize
float gridCellSize() const
Definition: SensorData.h:266
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:295
sqlite3_finalize
#define sqlite3_finalize
Definition: sqlite3ext.h:329
rtabmap::DBDriverSqlite3::getLastNodeIdsQuery
virtual void getLastNodeIdsQuery(std::set< int > &ids) const
Definition: DBDriverSqlite3.cpp:2372
rtabmap::DBDriverSqlite3::save2DMapQuery
virtual void save2DMapQuery(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriverSqlite3.cpp:5018
sqlite3_column_int
#define sqlite3_column_int
Definition: sqlite3ext.h:297
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::DBDriverSqlite3::getImagesMemoryUsedQuery
virtual long getImagesMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:630
rtabmap::DBDriverSqlite3::stepScanUpdate
void stepScanUpdate(sqlite3_stmt *ppStmt, int nodeId, const LaserScan &image) const
Definition: DBDriverSqlite3.cpp:5843
UASSERT
#define UASSERT(condition)
rtabmap::VWDictionary::setLastWordId
void setLastWordId(int id)
Definition: VWDictionary.h:96
sqlite3_column_text
#define sqlite3_column_text
Definition: sqlite3ext.h:305
rtabmap::GPS::altitude
const double & altitude() const
Definition: GPS.h:62
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
sqlite3_bind_zeroblob
#define sqlite3_bind_zeroblob
Definition: sqlite3ext.h:397
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::DBDriverSqlite3::updateOccupancyGridQuery
virtual void updateOccupancyGridQuery(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewpoint) const
Definition: DBDriverSqlite3.cpp:4579
sqlite3_reset
#define sqlite3_reset
Definition: sqlite3ext.h:353
rtabmap::Statistics::serializeData
static std::string serializeData(const std::map< std::string, float > &data)
Definition: Statistics.cpp:42
SQLITE_OPEN_CREATE
#define SQLITE_OPEN_CREATE
Definition: sqlite3.c:546
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::DBDriverSqlite3::updateLinkQuery
virtual void updateLinkQuery(const Link &link) const
Definition: DBDriverSqlite3.cpp:4552
rtabmap::VisualWord::isSaved
bool isSaved() const
Definition: VisualWord.h:53
rtabmap::DBDriverSqlite3::load2DMapQuery
virtual cv::Mat load2DMapQuery(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriverSqlite3.cpp:5070
rtabmap::SensorData::gps
const GPS & gps() const
Definition: SensorData.h:287
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::DBDriverSqlite3::getNodesObservingLandmarkQuery
virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map< int, Link > &nodes) const
Definition: DBDriverSqlite3.cpp:2710
rtabmap::Parameters::deserialize
static ParametersMap deserialize(const std::string &parameters)
Definition: Parameters.cpp:109
rtabmap::DBDriverSqlite3::getAllStatisticsWmStatesQuery
virtual std::map< int, std::vector< int > > getAllStatisticsWmStatesQuery() const
Definition: DBDriverSqlite3.cpp:1245
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::DBDriverSqlite3::stepImage
void stepImage(sqlite3_stmt *ppStmt, int id, const cv::Mat &imageBytes) const
Definition: DBDriverSqlite3.cpp:5627
SQLITE_OPEN_READWRITE
#define SQLITE_OPEN_READWRITE
Definition: sqlite3.c:545
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
uHex2Str
std::string UTILITE_EXP uHex2Str(const std::string &hex)
Definition: UConversion.cpp:250
rtabmap::DBDriverSqlite3::queryStepGlobalDescriptor
std::string queryStepGlobalDescriptor() const
Definition: DBDriverSqlite3.cpp:6535
sqlite3_bind_double
#define sqlite3_bind_double
Definition: sqlite3ext.h:272
rtabmap::DBDriverSqlite3::updateDepthImageQuery
virtual void updateDepthImageQuery(int nodeId, const cv::Mat &image) const
Definition: DBDriverSqlite3.cpp:4618
rtabmap::DBDriverSqlite3::loadSignaturesQuery
virtual void loadSignaturesQuery(const std::list< int > &ids, std::list< Signature *> &signatures) const
Definition: DBDriverSqlite3.cpp:2895
rtabmap::compressData
std::vector< unsigned char > RTABMAP_EXP compressData(const cv::Mat &data)
Definition: Compression.cpp:170
rtabmap::SensorData::envSensors
const EnvSensors & envSensors() const
Definition: SensorData.h:294
rtabmap::VisualWord::id
int id() const
Definition: VisualWord.h:49
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::DBDriverSqlite3::executeNoResultQuery
virtual void executeNoResultQuery(const std::string &sql) const
Definition: DBDriverSqlite3.cpp:513
rtabmap::DBDriverSqlite3::stepDepthUpdate
void stepDepthUpdate(sqlite3_stmt *ppStmt, int nodeId, const cv::Mat &imageCompressed) const
Definition: DBDriverSqlite3.cpp:5785
rtabmap::DBDriverSqlite3::getLastIdQuery
virtual void getLastIdQuery(const std::string &tableName, int &id, const std::string &fieldName="id") const
Definition: DBDriverSqlite3.cpp:2622
rtabmap::DBDriverSqlite3::queryStepDepth
std::string queryStepDepth() const
Definition: DBDriverSqlite3.cpp:5662
rtabmap::DBDriverSqlite3::getStatisticsMemoryUsedQuery
virtual long getStatisticsMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:917
sqlite3_bind_null
#define sqlite3_bind_null
Definition: sqlite3ext.h:275
rtabmap::DBDriverSqlite3::queryStepWordsChanged
std::string queryStepWordsChanged() const
Definition: DBDriverSqlite3.cpp:6387
rtabmap::DBDriverSqlite3::_memoryUsedEstimate
unsigned long _memoryUsedEstimate
Definition: DBDriverSqlite3.h:192
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:117
UTimer::start
void start()
Definition: UTimer.cpp:87
rtabmap::compressData2
cv::Mat RTABMAP_EXP compressData2(const cv::Mat &data)
Definition: Compression.cpp:201
rtabmap::DBDriverSqlite3::loadPreviewImageQuery
virtual cv::Mat loadPreviewImageQuery() const
Definition: DBDriverSqlite3.cpp:4811
sqlite3_close
#define sqlite3_close
Definition: sqlite3ext.h:285
rtabmap::DBDriverSqlite3::queryStepNode
std::string queryStepNode() const
Definition: DBDriverSqlite3.cpp:5461
rtabmap::DBDriverSqlite3::getCalibrationQuery
virtual bool getCalibrationQuery(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriverSqlite3.cpp:1859
sqlite3_column_type
#define sqlite3_column_type
Definition: sqlite3ext.h:307
sqlite3_bind_blob
#define sqlite3_bind_blob
Definition: sqlite3ext.h:271
rtabmap::DBDriverSqlite3::loadNodeDataQuery
virtual void loadNodeDataQuery(std::list< Signature *> &signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriverSqlite3.cpp:1295
rtabmap::Transform::normalizeRotation
void normalizeRotation()
Definition: Transform.cpp:306
sqlite3_column_double
#define sqlite3_column_double
Definition: sqlite3ext.h:296
rtabmap::DBDriverSqlite3::queryStepDepthUpdate
std::string queryStepDepthUpdate() const
Definition: DBDriverSqlite3.cpp:5774
rtabmap::DBDriverSqlite3::loadLastNodesQuery
virtual void loadLastNodesQuery(std::list< Signature *> &signatures) const
Definition: DBDriverSqlite3.cpp:3534
rtabmap::DBDriverSqlite3::stepGlobalDescriptor
void stepGlobalDescriptor(sqlite3_stmt *ppStmt, int nodeId, const GlobalDescriptor &descriptor) const
Definition: DBDriverSqlite3.cpp:6540
rtabmap::SensorData::depthOrRightCompressed
const cv::Mat & depthOrRightCompressed() const
Definition: SensorData.h:180
rtabmap::DBDriverSqlite3::DBDriverSqlite3
DBDriverSqlite3(const ParametersMap &parameters=ParametersMap())
Definition: DBDriverSqlite3.cpp:51
rtabmap::DBDriverSqlite3::getAllStatisticsQuery
virtual std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatisticsQuery() const
Definition: DBDriverSqlite3.cpp:1188
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DBDriverSqlite3::queryStepImage
std::string queryStepImage() const
Definition: DBDriverSqlite3.cpp:5622
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:120
rtabmap::DBDriverSqlite3::disconnectDatabaseQuery
virtual void disconnectDatabaseQuery(bool save=true, const std::string &outputUrl="")
Definition: DBDriverSqlite3.cpp:447
rtabmap::GlobalDescriptor::data
const cv::Mat data() const
Definition: GlobalDescriptor.h:51
rtabmap::DBDriverSqlite3::getGridsMemoryUsedQuery
virtual long getGridsMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:731
rtabmap::DBDriverSqlite3::isConnectedQuery
virtual bool isConnectedQuery() const
Definition: DBDriverSqlite3.cpp:507
sqlite3_prepare_v2
#define sqlite3_prepare_v2
Definition: sqlite3ext.h:394
rtabmap::DBDriverSqlite3::getWeightQuery
virtual void getWeightQuery(int signatureId, int &weight) const
Definition: DBDriverSqlite3.cpp:2860
rtabmap::SensorData::gridViewPoint
const cv::Point3f & gridViewPoint() const
Definition: SensorData.h:267
sqlite3_next_stmt
#define sqlite3_next_stmt
Definition: sqlite3ext.h:429
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:993
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::DBDriverSqlite3::loadOptimizedMeshQuery
virtual cv::Mat loadOptimizedMeshQuery(std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons, std::vector< std::vector< Eigen::Vector2f > > *texCoords, cv::Mat *textures) const
Definition: DBDriverSqlite3.cpp:5324
rtabmap::DBDriverSqlite3::setDbInMemory
void setDbInMemory(bool dbInMemory)
Definition: DBDriverSqlite3.cpp:198
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::compressString
cv::Mat RTABMAP_EXP compressString(const std::string &str)
Definition: Compression.cpp:277
rtabmap::DBDriverSqlite3::stepLink
void stepLink(sqlite3_stmt *ppStmt, const Link &link) const
Definition: DBDriverSqlite3.cpp:6317
rtabmap::GPS::longitude
const double & longitude() const
Definition: GPS.h:60
UERROR
#define UERROR(...)
rtabmap::DBDriverSqlite3::getAllLinksQuery
virtual void getAllLinksQuery(std::multimap< int, Link > &links, bool ignoreNullLinks, bool withLandmarks) const
Definition: DBDriverSqlite3.cpp:2486
rtabmap::SensorData::userDataCompressed
const cv::Mat & userDataCompressed() const
Definition: SensorData.h:249
rtabmap::Statistics::refImageId
int refImageId() const
Definition: Statistics.h:267
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:132
sqlite3_bind_int
#define sqlite3_bind_int
Definition: sqlite3ext.h:273
rtabmap::DBDriverSqlite3::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: DBDriverSqlite3.cpp:71
rtabmap::DBDriverSqlite3::queryStepOccupancyGridUpdate
std::string queryStepOccupancyGridUpdate() const
Definition: DBDriverSqlite3.cpp:6590
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:132
rtabmap::DBDriverSqlite3::getLastDictionarySizeQuery
virtual int getLastDictionarySizeQuery() const
Definition: DBDriverSqlite3.cpp:985
UWARN
#define UWARN(...)
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::uncompressString
std::string RTABMAP_EXP uncompressString(const cv::Mat &bytes)
Definition: Compression.cpp:283
rtabmap::DBDriverSqlite3::getDatabaseVersionQuery
virtual bool getDatabaseVersionQuery(std::string &version) const
Definition: DBDriverSqlite3.cpp:283
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::CameraModel::deserialize
unsigned int deserialize(const std::vector< unsigned char > &data)
Definition: CameraModel.cpp:535
rtabmap::Signature::getStamp
double getStamp() const
Definition: Signature.h:79
rtabmap::DBDriverSqlite3::getMemoryUsedQuery
virtual unsigned long getMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:526
rtabmap::DBDriverSqlite3::getLaserScansMemoryUsedQuery
virtual long getLaserScansMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:768
rtabmap::DBDriverSqlite3::queryStepLink
std::string queryStepLink() const
Definition: DBDriverSqlite3.cpp:6293
SQLITE_OK
#define SQLITE_OK
Definition: sqlite3.c:428
sqlite3_open_v2
#define sqlite3_open_v2
Definition: sqlite3ext.h:412
rtabmap::DBDriverSqlite3::saveQuery
virtual void saveQuery(const std::list< Signature *> &signatures)
Definition: DBDriverSqlite3.cpp:4281
rtabmap::DBDriverSqlite3::saveOptimizedPosesQuery
virtual void saveOptimizedPosesQuery(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriverSqlite3.cpp:4862
trace.model
model
Definition: trace.py:4
rtabmap::DBDriverSqlite3::stepNode
void stepNode(sqlite3_stmt *ppStmt, const Signature *s) const
Definition: DBDriverSqlite3.cpp:5493
UFile::rename
static int rename(const std::string &oldFilePath, const std::string &newFilePath)
Definition: UFile.cpp:63
rtabmap::DBDriverSqlite3::stepDepth
void stepDepth(sqlite3_stmt *ppStmt, const SensorData &sensorData) const
Definition: DBDriverSqlite3.cpp:5678
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
rtabmap::DBDriverSqlite3::getNodeInfoQuery
virtual bool getNodeInfoQuery(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors) const
Definition: DBDriverSqlite3.cpp:2211
rtabmap::DBDriverSqlite3::setSynchronous
void setSynchronous(int synchronous)
Definition: DBDriverSqlite3.cpp:142
rtabmap::GPS::latitude
const double & latitude() const
Definition: GPS.h:61
rtabmap::DBDriverSqlite3::connectDatabaseQuery
virtual bool connectDatabaseQuery(const std::string &url, bool overwritten=false)
Definition: DBDriverSqlite3.cpp:321
rtabmap::DBDriverSqlite3::getLastNodesSizeQuery
virtual int getLastNodesSizeQuery() const
Definition: DBDriverSqlite3.cpp:953
sqlite3_column_bytes
#define sqlite3_column_bytes
Definition: sqlite3ext.h:289
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
rtabmap::DBDriverSqlite3::stepWordsChanged
void stepWordsChanged(sqlite3_stmt *ppStmt, int signatureId, int oldWordId, int newWordId) const
Definition: DBDriverSqlite3.cpp:6398
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:121
sqlite3_backup_step
#define sqlite3_backup_step
Definition: sqlite3ext.h:436
rtabmap::VisualWord::setSaved
void setSaved(bool saved)
Definition: VisualWord.h:54
rtabmap::Statistics::deserializeData
static std::map< std::string, float > deserializeData(const std::string &data)
Definition: Statistics.cpp:57
rtabmap::DBDriverSqlite3::stepOccupancyGridUpdate
void stepOccupancyGridUpdate(sqlite3_stmt *ppStmt, int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewpoint) const
Definition: DBDriverSqlite3.cpp:6599
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::DBDriverSqlite3::getLaserScanInfoQuery
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan &info) const
Definition: DBDriverSqlite3.cpp:2103
sqlite3_bind_text
#define sqlite3_bind_text
Definition: sqlite3ext.h:279
rtabmap::VWDictionary::addWord
virtual void addWord(VisualWord *vw)
Definition: VWDictionary.cpp:1393
rtabmap::DBDriverSqlite3::queryStepLinkUpdate
std::string queryStepLinkUpdate() const
Definition: DBDriverSqlite3.cpp:6270
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::DBDriverSqlite3::getLinksMemoryUsedQuery
virtual long getLinksMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:586
rtabmap::SensorData::gridEmptyCellsCompressed
const cv::Mat & gridEmptyCellsCompressed() const
Definition: SensorData.h:265
rtabmap::DBDriverSqlite3::loadQuery
virtual void loadQuery(VWDictionary *dictionary, bool lastStateOnly=true) const
Definition: DBDriverSqlite3.cpp:3585
SQLITE_NULL
#define SQLITE_NULL
Definition: sqlite3.c:3718
rtabmap::DBDriverSqlite3::stepSensorData
void stepSensorData(sqlite3_stmt *ppStmt, const SensorData &sensorData) const
Definition: DBDriverSqlite3.cpp:5977
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::DBDriverSqlite3::getCalibrationsMemoryUsedQuery
virtual long getCalibrationsMemoryUsedQuery() const
Definition: DBDriverSqlite3.cpp:694
rtabmap::DBDriver::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: DBDriver.cpp:59
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:179
rtabmap::DBDriverSqlite3::updateQuery
virtual void updateQuery(const std::list< Signature *> &signatures, bool updateTimestamp) const
Definition: DBDriverSqlite3.cpp:4091
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::DBDriverSqlite3::queryStepKeypoint
std::string queryStepKeypoint() const
Definition: DBDriverSqlite3.cpp:6420
rtabmap::VisualWord::getDescriptor
const cv::Mat & getDescriptor() const
Definition: VisualWord.h:50
rtabmap::DBDriverSqlite3::queryStepScanUpdate
std::string queryStepScanUpdate() const
Definition: DBDriverSqlite3.cpp:5827
rtabmap::DBDriver::isConnected
bool isConnected() const
Definition: DBDriver.cpp:100
rtabmap::DBDriver::emptyTrashes
void emptyTrashes(bool async=false)
Definition: DBDriver.cpp:311
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Statistics::stamp
double stamp() const
Definition: Statistics.h:273
rtabmap::DBDriverSqlite3::getAllLabelsQuery
virtual void getAllLabelsQuery(std::map< int, std::string > &labels) const
Definition: DBDriverSqlite3.cpp:2820
rtabmap::uncompressImage
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:130
rtabmap::DBDriverSqlite3::getNodeIdByLabelQuery
virtual void getNodeIdByLabelQuery(const std::string &label, int &id) const
Definition: DBDriverSqlite3.cpp:2790
sqlite3_column_blob
#define sqlite3_column_blob
Definition: sqlite3ext.h:288
SQLITE_STATIC
#define SQLITE_STATIC
Definition: sqlite3.c:4301
rtabmap::DBDriverSqlite3::updateLaserScanQuery
void updateLaserScanQuery(int nodeId, const LaserScan &scan) const
Definition: DBDriverSqlite3.cpp:4649
rtabmap::compressImage2
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat &image, const std::string &format=".png")
Definition: Compression.cpp:120
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28