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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58