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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:08