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  // use to_id to ignore all children (which don't have link pointing on them)
2431  // ignore self referring links
2432  // keep nodes without link to other nodes (map has only a single node)
2433  query << "WHERE ";
2434  query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
2435  query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
2436 
2437  query << "AND weight>-9 "; //ignore invalid nodes
2438  if(ignoreIntermediateNodes)
2439  {
2440  query << "AND weight!=-1 "; //ignore intermediate nodes
2441  }
2442  }
2443  else if(ignoreIntermediateNodes)
2444  {
2445  query << "WHERE weight!=-1 "; //ignore intermediate nodes
2446  }
2447 
2448  if(ignoreBadSignatures)
2449  {
2450  if(ignoreChildren || ignoreIntermediateNodes)
2451  {
2452  query << "AND ";
2453  }
2454  else
2455  {
2456  query << "WHERE ";
2457  }
2458  if(uStrNumCmp(_version, "0.13.0") >= 0)
2459  {
2460  query << " id in (select node_id from Feature) ";
2461  }
2462  else
2463  {
2464  query << " id in (select node_id from Map_Node_Word) ";
2465  }
2466  }
2467 
2468  query << "ORDER BY id";
2469 
2470  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2471  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2472 
2473 
2474  // Process the result if one
2475  rc = sqlite3_step(ppStmt);
2476  while(rc == SQLITE_ROW)
2477  {
2478  ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
2479  rc = sqlite3_step(ppStmt);
2480  }
2481  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2482 
2483  // Finalize (delete) the statement
2484  rc = sqlite3_finalize(ppStmt);
2485  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2486  ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
2487  }
2488 }
2489 
2490 void DBDriverSqlite3::getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
2491 {
2492  if(_ppDb)
2493  {
2494  UTimer timer;
2495  timer.start();
2496  int rc = SQLITE_OK;
2497  sqlite3_stmt * ppStmt = 0;
2498  std::stringstream query;
2499 
2500  query << "SELECT DISTINCT id, pose "
2501  << "FROM Node ";
2502  if(ignoreChildren)
2503  {
2504  query << "INNER JOIN Link ";
2505  query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
2506  query << "WHERE from_id != to_id "; // ignore self referring links
2507  query << "AND weight>-9 "; //ignore invalid nodes
2508  if(ignoreIntermediateNodes)
2509  {
2510  query << "AND weight!=-1 "; //ignore intermediate nodes
2511  }
2512  }
2513  else if(ignoreIntermediateNodes)
2514  {
2515  query << "WHERE weight!=-1 "; //ignore intermediate nodes
2516  }
2517 
2518  query << "ORDER BY id";
2519 
2520  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2521  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2522 
2523  const void * data = 0;
2524  int dataSize = 0;
2525 
2526  // Process the result if one
2527  rc = sqlite3_step(ppStmt);
2528  while(rc == SQLITE_ROW)
2529  {
2530  int id = sqlite3_column_int(ppStmt, 0); // Signature Id
2531  data = sqlite3_column_blob(ppStmt, 1); // Pose
2532  dataSize = sqlite3_column_bytes(ppStmt, 1);
2533 
2534  Transform pose;
2535  if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
2536  {
2537  memcpy(pose.data(), data, dataSize);
2538  if(uStrNumCmp(_version, "0.15.2") < 0)
2539  {
2540  pose.normalizeRotation();
2541  }
2542  }
2543  else if(dataSize)
2544  {
2545  UERROR("Error while loading pose for node %d! Setting to null...", id);
2546  }
2547  poses.insert(std::make_pair(id, pose));
2548  rc = sqlite3_step(ppStmt);
2549  }
2550  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2551 
2552  // Finalize (delete) the statement
2553  rc = sqlite3_finalize(ppStmt);
2554  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2555  ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)poses.size());
2556  }
2557 }
2558 
2559 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
2560 {
2561  links.clear();
2562  if(_ppDb)
2563  {
2564  UTimer timer;
2565  timer.start();
2566  int rc = SQLITE_OK;
2567  sqlite3_stmt * ppStmt = 0;
2568  std::stringstream query;
2569 
2570  if(uStrNumCmp(_version, "0.18.3") >= 0 && !withLandmarks)
2571  {
2572  query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link WHERE type!=" << Link::kLandmark << " ORDER BY from_id, to_id";
2573  }
2574  else if(uStrNumCmp(_version, "0.13.0") >= 0)
2575  {
2576  query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
2577  }
2578  else if(uStrNumCmp(_version, "0.10.10") >= 0)
2579  {
2580  query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
2581  }
2582  else if(uStrNumCmp(_version, "0.8.4") >= 0)
2583  {
2584  query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
2585  }
2586  else if(uStrNumCmp(_version, "0.7.4") >= 0)
2587  {
2588  query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
2589  }
2590  else
2591  {
2592  query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
2593  }
2594 
2595  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2596  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2597 
2598  int fromId = -1;
2599  int toId = -1;
2600  int type = Link::kUndef;
2601  const void * data = 0;
2602  int dataSize = 0;
2603 
2604  // Process the result if one
2605  rc = sqlite3_step(ppStmt);
2606  while(rc == SQLITE_ROW)
2607  {
2608  int index = 0;
2609 
2610  fromId = sqlite3_column_int(ppStmt, index++);
2611  toId = sqlite3_column_int(ppStmt, index++);
2612  type = sqlite3_column_int(ppStmt, index++);
2613 
2614  data = sqlite3_column_blob(ppStmt, index);
2615  dataSize = sqlite3_column_bytes(ppStmt, index++);
2616 
2617  Transform transform;
2618  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
2619  {
2620  memcpy(transform.data(), data, dataSize);
2621  if(uStrNumCmp(_version, "0.15.2") < 0)
2622  {
2623  transform.normalizeRotation();
2624  }
2625  }
2626  else if(dataSize)
2627  {
2628  UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
2629  }
2630 
2631  if(!ignoreNullLinks || !transform.isNull())
2632  {
2633  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
2634  if(uStrNumCmp(_version, "0.8.4") >= 0)
2635  {
2636  if(uStrNumCmp(_version, "0.13.0") >= 0)
2637  {
2638  data = sqlite3_column_blob(ppStmt, index);
2639  dataSize = sqlite3_column_bytes(ppStmt, index++);
2640  UASSERT(dataSize==36*sizeof(double) && data);
2641  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
2642  }
2643  else
2644  {
2645  double rotVariance = sqlite3_column_double(ppStmt, index++);
2646  double transVariance = sqlite3_column_double(ppStmt, index++);
2647  UASSERT(rotVariance > 0.0 && transVariance>0.0);
2648  informationMatrix.at<double>(0,0) = 1.0/transVariance;
2649  informationMatrix.at<double>(1,1) = 1.0/transVariance;
2650  informationMatrix.at<double>(2,2) = 1.0/transVariance;
2651  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
2652  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
2653  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
2654  }
2655 
2656  cv::Mat userDataCompressed;
2657  if(uStrNumCmp(_version, "0.10.10") >= 0)
2658  {
2659  const void * data = sqlite3_column_blob(ppStmt, index);
2660  dataSize = sqlite3_column_bytes(ppStmt, index++);
2661  //Create the userData
2662  if(dataSize>4 && data)
2663  {
2664  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
2665  }
2666  }
2667 
2668  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
2669  }
2670  else if(uStrNumCmp(_version, "0.7.4") >= 0)
2671  {
2672  double variance = sqlite3_column_double(ppStmt, index++);
2673  UASSERT(variance>0.0);
2674  informationMatrix *= 1.0/variance;
2675  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
2676  }
2677  else
2678  {
2679  // neighbor is 0, loop closures are 1 and 2 (child)
2680  links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
2681  }
2682  }
2683 
2684  rc = sqlite3_step(ppStmt);
2685  }
2686 
2687  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2688 
2689  // Finalize (delete) the statement
2690  rc = sqlite3_finalize(ppStmt);
2691  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2692  }
2693 }
2694 
2695 void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName) const
2696 {
2697  if(_ppDb)
2698  {
2699  UDEBUG("get last %s from table \"%s\"", fieldName.c_str(), tableName.c_str());
2700  UTimer timer;
2701  timer.start();
2702  int rc = SQLITE_OK;
2703  sqlite3_stmt * ppStmt = 0;
2704  std::stringstream query;
2705 
2706  query << "SELECT COALESCE(MAX(" << fieldName << "), " << id << ") " // In case the table is empty, return back input id
2707  << "FROM " << tableName
2708  << ";";
2709 
2710  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2711  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2712 
2713 
2714  // Process the result if one
2715  rc = sqlite3_step(ppStmt);
2716  if(rc == SQLITE_ROW)
2717  {
2718  id = sqlite3_column_int(ppStmt, 0); // Signature Id
2719  rc = sqlite3_step(ppStmt);
2720  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2721  }
2722  else
2723  {
2724  UDEBUG("No result from the DB for table %s with field %s", tableName.c_str(), fieldName.c_str());
2725  }
2726 
2727  // Finalize (delete) the statement
2728  rc = sqlite3_finalize(ppStmt);
2729  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2730  ULOGGER_DEBUG("Time=%fs", timer.ticks());
2731  }
2732 }
2733 
2734 void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
2735 {
2736  ni = 0;
2737  if(_ppDb)
2738  {
2739  UTimer timer;
2740  timer.start();
2741  int rc = SQLITE_OK;
2742  sqlite3_stmt * ppStmt = 0;
2743  std::stringstream query;
2744 
2745  if(uStrNumCmp(_version, "0.13.0") >= 0)
2746  {
2747  query << "SELECT count(word_id) "
2748  << "FROM Feature "
2749  << "WHERE node_id=" << nodeId << ";";
2750  }
2751  else
2752  {
2753  query << "SELECT count(word_id) "
2754  << "FROM Map_Node_Word "
2755  << "WHERE node_id=" << nodeId << ";";
2756  }
2757 
2758  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2759  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2760 
2761 
2762  // Process the result if one
2763  rc = sqlite3_step(ppStmt);
2764  if(rc == SQLITE_ROW)
2765  {
2766  ni = sqlite3_column_int(ppStmt, 0);
2767  rc = sqlite3_step(ppStmt);
2768  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2769  }
2770  else
2771  {
2772  ULOGGER_ERROR("No result !?! from the DB, node=%d",nodeId);
2773  }
2774 
2775 
2776  // Finalize (delete) the statement
2777  rc = sqlite3_finalize(ppStmt);
2778  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2779  ULOGGER_DEBUG("Time=%fs", timer.ticks());
2780  }
2781 }
2782 
2783 void DBDriverSqlite3::getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const
2784 {
2785  if(_ppDb && landmarkId < 0 && uStrNumCmp(_version, "0.18.3") >= 0)
2786  {
2787  UTimer timer;
2788  timer.start();
2789  int rc = SQLITE_OK;
2790  sqlite3_stmt * ppStmt = 0;
2791  std::stringstream query;
2792 
2793  query << "SELECT from_id, type, information_matrix, transform, user_data FROM Link WHERE to_id=" << landmarkId <<"";
2794 
2795  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2796  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2797 
2798  // Process the result if one
2799  int fromId = -1;
2800  int linkType = -1;
2801  std::list<Link> links;
2802  const void * data = 0;
2803  int dataSize = 0;
2804 
2805  // Process the result if one
2806  rc = sqlite3_step(ppStmt);
2807  while(rc == SQLITE_ROW)
2808  {
2809  int index = 0;
2810 
2811  fromId = sqlite3_column_int(ppStmt, index++);
2812  linkType = sqlite3_column_int(ppStmt, index++);
2813  cv::Mat userDataCompressed;
2814  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
2815 
2816  data = sqlite3_column_blob(ppStmt, index);
2817  dataSize = sqlite3_column_bytes(ppStmt, index++);
2818  UASSERT(dataSize==36*sizeof(double) && data);
2819  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
2820 
2821  const void * data = sqlite3_column_blob(ppStmt, index);
2822  dataSize = sqlite3_column_bytes(ppStmt, index++);
2823  //Create the userData
2824  if(dataSize>4 && data)
2825  {
2826  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
2827  }
2828 
2829  //transform
2830  data = sqlite3_column_blob(ppStmt, index);
2831  dataSize = sqlite3_column_bytes(ppStmt, index++);
2832  Transform transform;
2833  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
2834  {
2835  memcpy(transform.data(), data, dataSize);
2836  }
2837  else if(dataSize)
2838  {
2839  UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, landmarkId);
2840  }
2841 
2842  if(linkType >= 0 && linkType != Link::kUndef)
2843  {
2844  nodes.insert(std::make_pair(fromId, Link(fromId, landmarkId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed)));
2845  }
2846  else
2847  {
2848  UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
2849  linkType, fromId, landmarkId);
2850  }
2851 
2852  rc = sqlite3_step(ppStmt);
2853  }
2854  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2855 
2856  // Finalize (delete) the statement
2857  rc = sqlite3_finalize(ppStmt);
2858  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2859  ULOGGER_DEBUG("Time=%f", timer.ticks());
2860  }
2861 }
2862 
2863 void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
2864 {
2865  if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
2866  {
2867  UTimer timer;
2868  timer.start();
2869  int rc = SQLITE_OK;
2870  sqlite3_stmt * ppStmt = 0;
2871  std::stringstream query;
2872  query << "SELECT id FROM Node WHERE label='" << label <<"'";
2873 
2874  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2875  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2876 
2877  // Process the result if one
2878  rc = sqlite3_step(ppStmt);
2879  if(rc == SQLITE_ROW)
2880  {
2881  id = sqlite3_column_int(ppStmt, 0);
2882  rc = sqlite3_step(ppStmt);
2883  }
2884  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2885 
2886  // Finalize (delete) the statement
2887  rc = sqlite3_finalize(ppStmt);
2888  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2889  ULOGGER_DEBUG("Time=%f", timer.ticks());
2890  }
2891 }
2892 
2893 void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
2894 {
2895  if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
2896  {
2897  UTimer timer;
2898  timer.start();
2899  int rc = SQLITE_OK;
2900  sqlite3_stmt * ppStmt = 0;
2901  std::stringstream query;
2902  query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
2903 
2904  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2905  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2906 
2907  // Process the result if one
2908  rc = sqlite3_step(ppStmt);
2909  while(rc == SQLITE_ROW)
2910  {
2911  int index = 0;
2912  int id = sqlite3_column_int(ppStmt, index++);
2913  const unsigned char * p = sqlite3_column_text(ppStmt, index++);
2914  if(p)
2915  {
2916  std::string label = reinterpret_cast<const char*>(p);
2917  if(!label.empty())
2918  {
2919  labels.insert(std::make_pair(id, label));
2920  }
2921  }
2922  rc = sqlite3_step(ppStmt);
2923  }
2924  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2925 
2926  // Finalize (delete) the statement
2927  rc = sqlite3_finalize(ppStmt);
2928  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2929  ULOGGER_DEBUG("Time=%f", timer.ticks());
2930  }
2931 }
2932 
2933 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
2934 {
2935  weight = 0;
2936  if(_ppDb)
2937  {
2938  UTimer timer;
2939  timer.start();
2940  int rc = SQLITE_OK;
2941  sqlite3_stmt * ppStmt = 0;
2942  std::stringstream query;
2943 
2944  query << "SELECT weight FROM node WHERE id = "
2945  << nodeId
2946  << ";";
2947 
2948  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
2949  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2950 
2951 
2952  // Process the result if one
2953  rc = sqlite3_step(ppStmt);
2954  if(rc == SQLITE_ROW)
2955  {
2956  weight= sqlite3_column_int(ppStmt, 0); // weight
2957  rc = sqlite3_step(ppStmt);
2958  }
2959  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2960 
2961  // Finalize (delete) the statement
2962  rc = sqlite3_finalize(ppStmt);
2963  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
2964  }
2965 }
2966 
2967 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
2968 void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
2969 {
2970  ULOGGER_DEBUG("count=%d", (int)ids.size());
2971  if(_ppDb && ids.size())
2972  {
2973  std::string type;
2974  UTimer timer;
2975  timer.start();
2976  int rc = SQLITE_OK;
2977  sqlite3_stmt * ppStmt = 0;
2978  std::stringstream query;
2979  unsigned int loaded = 0;
2980 
2981  // Load nodes information
2982  if(uStrNumCmp(_version, "0.18.0") >= 0)
2983  {
2984  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors "
2985  << "FROM Node "
2986  << "WHERE id=?;";
2987  }
2988  else if(uStrNumCmp(_version, "0.14.0") >= 0)
2989  {
2990  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps "
2991  << "FROM Node "
2992  << "WHERE id=?;";
2993  }
2994  else if(uStrNumCmp(_version, "0.13.0") >= 0)
2995  {
2996  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
2997  << "FROM Node "
2998  << "WHERE id=?;";
2999  }
3000  else if(uStrNumCmp(_version, "0.11.1") >= 0)
3001  {
3002  query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
3003  << "FROM Node "
3004  << "WHERE id=?;";
3005  }
3006  else if(uStrNumCmp(_version, "0.8.5") >= 0)
3007  {
3008  query << "SELECT id, map_id, weight, pose, stamp, label "
3009  << "FROM Node "
3010  << "WHERE id=?;";
3011  }
3012  else
3013  {
3014  query << "SELECT id, map_id, weight, pose "
3015  << "FROM Node "
3016  << "WHERE id=?;";
3017  }
3018 
3019  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3020  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3021 
3022  for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
3023  {
3024  //ULOGGER_DEBUG("Loading node %d...", *iter);
3025  // bind id
3026  rc = sqlite3_bind_int(ppStmt, 1, *iter);
3027  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3028 
3029  int id = 0;
3030  int mapId = 0;
3031  double stamp = 0.0;
3032  int weight = 0;
3033  Transform pose;
3034  Transform groundTruthPose;
3035  std::vector<float> velocity;
3036  std::vector<double> gps;
3037  EnvSensors sensors;
3038  const void * data = 0;
3039  int dataSize = 0;
3040  std::string label;
3041 
3042  // Process the result if one
3043  rc = sqlite3_step(ppStmt);
3044  if(rc == SQLITE_ROW)
3045  {
3046  int index = 0;
3047  id = sqlite3_column_int(ppStmt, index++); // Signature Id
3048  mapId = sqlite3_column_int(ppStmt, index++); // Map Id
3049  weight = sqlite3_column_int(ppStmt, index++); // weight
3050 
3051  data = sqlite3_column_blob(ppStmt, index); // pose
3052  dataSize = sqlite3_column_bytes(ppStmt, index++);
3053  if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
3054  {
3055  memcpy(pose.data(), data, dataSize);
3056  if(uStrNumCmp(_version, "0.15.2") < 0)
3057  {
3058  pose.normalizeRotation();
3059  }
3060  }
3061 
3062  if(uStrNumCmp(_version, "0.8.5") >= 0)
3063  {
3064  stamp = sqlite3_column_double(ppStmt, index++); // stamp
3065  const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
3066  if(p)
3067  {
3068  label = reinterpret_cast<const char*>(p);
3069  }
3070 
3071  if(uStrNumCmp(_version, "0.11.1") >= 0)
3072  {
3073  data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
3074  dataSize = sqlite3_column_bytes(ppStmt, index++);
3075  if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
3076  {
3077  memcpy(groundTruthPose.data(), data, dataSize);
3078  if(uStrNumCmp(_version, "0.15.2") < 0)
3079  {
3080  groundTruthPose.normalizeRotation();
3081  }
3082  }
3083 
3084  if(uStrNumCmp(_version, "0.13.0") >= 0)
3085  {
3086  velocity.resize(6,0);
3087  data = sqlite3_column_blob(ppStmt, index); // velocity
3088  dataSize = sqlite3_column_bytes(ppStmt, index++);
3089  if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
3090  {
3091  memcpy(velocity.data(), data, dataSize);
3092  }
3093  }
3094 
3095  if(uStrNumCmp(_version, "0.14.0") >= 0)
3096  {
3097  gps.resize(6,0);
3098  data = sqlite3_column_blob(ppStmt, index); // gps
3099  dataSize = sqlite3_column_bytes(ppStmt, index++);
3100  if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
3101  {
3102  memcpy(gps.data(), data, dataSize);
3103  }
3104 
3105  if(uStrNumCmp(_version, "0.18.0") >= 0)
3106  {
3107  data = sqlite3_column_blob(ppStmt, index); // env_sensors
3108  dataSize = sqlite3_column_bytes(ppStmt, index++);
3109  if(data)
3110  {
3111  UASSERT(dataSize%sizeof(double)==0 && (dataSize/sizeof(double))%3 == 0);
3112  const double * dataDouble = (const double *)data;
3113  int sensorsNum = (dataSize/sizeof(double))/3;
3114  for(int i=0; i<sensorsNum;++i)
3115  {
3116  EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
3117  sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
3118  }
3119  }
3120  }
3121  }
3122  }
3123  }
3124 
3125  rc = sqlite3_step(ppStmt);
3126  }
3127  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3128 
3129  // create the node
3130  if(id)
3131  {
3132  ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
3133  Signature * s = new Signature(
3134  id,
3135  mapId,
3136  weight,
3137  stamp,
3138  label,
3139  pose,
3140  groundTruthPose);
3141  if(velocity.size() == 6)
3142  {
3143  s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
3144  }
3145  if(gps.size() == 6)
3146  {
3147  s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
3148  }
3149  s->sensorData().setEnvSensors(sensors);
3150  s->setSaved(true);
3151  nodes.push_back(s);
3152  ++loaded;
3153  }
3154  else
3155  {
3156  UERROR("Signature %d not found in database!", *iter);
3157  }
3158 
3159  //reset
3160  rc = sqlite3_reset(ppStmt);
3161  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3162  }
3163 
3164  // Finalize (delete) the statement
3165  rc = sqlite3_finalize(ppStmt);
3166  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3167 
3168  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3169 
3170  // Prepare the query... Get the map from signature and visual words
3171  std::stringstream query2;
3172  if(uStrNumCmp(_version, "0.13.0") >= 0)
3173  {
3174  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3175  "FROM Feature "
3176  "WHERE node_id = ? ";
3177  }
3178  else if(uStrNumCmp(_version, "0.12.0") >= 0)
3179  {
3180  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3181  "FROM Map_Node_Word "
3182  "WHERE node_id = ? ";
3183  }
3184  else if(uStrNumCmp(_version, "0.11.2") >= 0)
3185  {
3186  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
3187  "FROM Map_Node_Word "
3188  "WHERE node_id = ? ";
3189  }
3190  else
3191  {
3192  query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
3193  "FROM Map_Node_Word "
3194  "WHERE node_id = ? ";
3195  }
3196 
3197  query2 << " ORDER BY word_id"; // Needed for fast insertion below
3198  query2 << ";";
3199 
3200  rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
3201  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3202 
3203  float nanFloat = std::numeric_limits<float>::quiet_NaN ();
3204 
3205  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3206  {
3207  //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
3208  // bind id
3209  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3210  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3211 
3212  int visualWordId = 0;
3213  int descriptorSize = 0;
3214  const void * descriptor = 0;
3215  int dRealSize = 0;
3216  cv::KeyPoint kpt;
3217  std::multimap<int, int> visualWords;
3218  std::vector<cv::KeyPoint> visualWordsKpts;
3219  std::vector<cv::Point3f> visualWords3;
3220  cv::Mat descriptors;
3221  bool allWords3NaN = true;
3222  cv::Point3f depth(0,0,0);
3223 
3224  // Process the result if one
3225  rc = sqlite3_step(ppStmt);
3226  while(rc == SQLITE_ROW)
3227  {
3228  int index = 0;
3229  visualWordId = sqlite3_column_int(ppStmt, index++);
3230  kpt.pt.x = sqlite3_column_double(ppStmt, index++);
3231  kpt.pt.y = sqlite3_column_double(ppStmt, index++);
3232  kpt.size = sqlite3_column_int(ppStmt, index++);
3233  kpt.angle = sqlite3_column_double(ppStmt, index++);
3234  kpt.response = sqlite3_column_double(ppStmt, index++);
3235  if(uStrNumCmp(_version, "0.12.0") >= 0)
3236  {
3237  kpt.octave = sqlite3_column_int(ppStmt, index++);
3238  }
3239 
3240  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3241  {
3242  depth.x = nanFloat;
3243  ++index;
3244  }
3245  else
3246  {
3247  depth.x = sqlite3_column_double(ppStmt, index++);
3248  }
3249 
3250  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3251  {
3252  depth.y = nanFloat;
3253  ++index;
3254  }
3255  else
3256  {
3257  depth.y = sqlite3_column_double(ppStmt, index++);
3258  }
3259 
3260  if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
3261  {
3262  depth.z = nanFloat;
3263  ++index;
3264  }
3265  else
3266  {
3267  depth.z = sqlite3_column_double(ppStmt, index++);
3268  }
3269 
3270  visualWordsKpts.push_back(kpt);
3271  visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
3272  visualWords3.push_back(depth);
3273 
3274  if(allWords3NaN && util3d::isFinite(depth))
3275  {
3276  allWords3NaN = false;
3277  }
3278 
3279  if(uStrNumCmp(_version, "0.11.2") >= 0)
3280  {
3281  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3282  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3283  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3284 
3285  if(descriptor && descriptorSize>0 && dRealSize>0)
3286  {
3287  cv::Mat d;
3288  if(dRealSize == descriptorSize)
3289  {
3290  // CV_8U binary descriptors
3291  d = cv::Mat(1, descriptorSize, CV_8U);
3292  }
3293  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3294  {
3295  // CV_32F
3296  d = cv::Mat(1, descriptorSize, CV_32F);
3297  }
3298  else
3299  {
3300  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3301  }
3302 
3303  memcpy(d.data, descriptor, dRealSize);
3304 
3305  descriptors.push_back(d);
3306  }
3307  }
3308 
3309  rc = sqlite3_step(ppStmt);
3310  }
3311  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3312 
3313  if(visualWords.size()==0)
3314  {
3315  UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
3316  }
3317  else
3318  {
3319  if(allWords3NaN)
3320  {
3321  visualWords3.clear();
3322  }
3323  (*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
3324  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());
3325  }
3326 
3327  //reset
3328  rc = sqlite3_reset(ppStmt);
3329  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3330  }
3331 
3332  // Finalize (delete) the statement
3333  rc = sqlite3_finalize(ppStmt);
3334  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3335 
3336  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3337 
3338  this->loadLinksQuery(nodes);
3339  ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
3340 
3341  for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
3342  {
3343  (*iter)->setModified(false);
3344  }
3345 
3346  // load calibrations
3347  if(nodes.size() && uStrNumCmp(_version, "0.10.0") >= 0)
3348  {
3349  std::stringstream query3;
3350  query3 << "SELECT calibration "
3351  "FROM Data "
3352  "WHERE id = ? ";
3353 
3354  rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
3355  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3356 
3357  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3358  {
3359  // bind id
3360  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3361  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3362 
3363  rc = sqlite3_step(ppStmt);
3364  if(rc == SQLITE_ROW)
3365  {
3366  int index=0;
3367  const void * data = 0;
3368  int dataSize = 0;
3369  Transform localTransform;
3370  std::vector<CameraModel> models;
3371  std::vector<StereoCameraModel> stereoModels;
3372 
3373  // calibration
3374  data = sqlite3_column_blob(ppStmt, index);
3375  dataSize = sqlite3_column_bytes(ppStmt, index++);
3376  // multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
3377  // stereo [fx, fy, cx, cy, baseline, [width,height], local_transform] (5or7+12)*float
3378  if(dataSize > 0 && data)
3379  {
3380  if(uStrNumCmp(_version, "0.18.0") >= 0)
3381  {
3382  if(dataSize >= int(sizeof(int)*4))
3383  {
3384  const int * dataInt = (const int *)data;
3385  int type = dataInt[3];
3386  if(type == 0) // mono
3387  {
3388  CameraModel model;
3389  int bytesReadTotal = 0;
3390  unsigned int bytesRead = 0;
3391  while(bytesReadTotal < dataSize &&
3392  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
3393  {
3394  bytesReadTotal+=bytesRead;
3395  models.push_back(model);
3396  }
3397  UASSERT(bytesReadTotal == dataSize);
3398  }
3399  else if(type == 1) // stereo
3400  {
3401  StereoCameraModel model;
3402  int bytesReadTotal = 0;
3403  unsigned int bytesRead = 0;
3404  while(bytesReadTotal < dataSize &&
3405  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
3406  {
3407  bytesReadTotal+=bytesRead;
3408  stereoModels.push_back(model);
3409  }
3410  UASSERT(bytesReadTotal == dataSize);
3411  }
3412  else
3413  {
3414  UFATAL("Unknown calibration type %d", type);
3415  }
3416  }
3417  else
3418  {
3419  UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
3420  }
3421  }
3422  else
3423  {
3424  float * dataFloat = (float*)data;
3425  if(uStrNumCmp(_version, "0.11.2") >= 0 &&
3426  (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
3427  {
3428  int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
3429  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
3430  int max = cameraCount*(6+localTransform.size());
3431  for(int i=0; i<max; i+=6+localTransform.size())
3432  {
3433  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
3434  localTransform = Transform::getIdentity();
3435  memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
3436  if(uStrNumCmp(_version, "0.15.2") < 0)
3437  {
3438  localTransform.normalizeRotation();
3439  }
3440  models.push_back(CameraModel(
3441  (double)dataFloat[i],
3442  (double)dataFloat[i+1],
3443  (double)dataFloat[i+2],
3444  (double)dataFloat[i+3],
3445  localTransform,
3446  0,
3447  cv::Size(dataFloat[i+4], dataFloat[i+5])));
3448  UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
3449  dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
3450  localTransform.prettyPrint().c_str());
3451  }
3452  }
3453  else if(uStrNumCmp(_version, "0.11.2") < 0 &&
3454  (unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
3455  {
3456  int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
3457  UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
3458  int max = cameraCount*(4+localTransform.size());
3459  for(int i=0; i<max; i+=4+localTransform.size())
3460  {
3461  // Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
3462  localTransform = Transform::getIdentity();
3463  memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
3464  if(uStrNumCmp(_version, "0.15.2") < 0)
3465  {
3466  localTransform.normalizeRotation();
3467  }
3468  models.push_back(CameraModel(
3469  (double)dataFloat[i],
3470  (double)dataFloat[i+1],
3471  (double)dataFloat[i+2],
3472  (double)dataFloat[i+3],
3473  localTransform));
3474  }
3475  }
3476  else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
3477  {
3478  UDEBUG("Loading calibration of a stereo camera");
3479  memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
3480  if(uStrNumCmp(_version, "0.15.2") < 0)
3481  {
3482  localTransform.normalizeRotation();
3483  }
3484  stereoModels.push_back(StereoCameraModel(
3485  dataFloat[0], // fx
3486  dataFloat[1], // fy
3487  dataFloat[2], // cx
3488  dataFloat[3], // cy
3489  dataFloat[4], // baseline
3490  localTransform,
3491  cv::Size(dataFloat[5], dataFloat[6])));
3492  }
3493  else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
3494  {
3495  UDEBUG("Loading calibration of a stereo camera");
3496  memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
3497  if(uStrNumCmp(_version, "0.15.2") < 0)
3498  {
3499  localTransform.normalizeRotation();
3500  }
3501  stereoModels.push_back(StereoCameraModel(
3502  dataFloat[0], // fx
3503  dataFloat[1], // fy
3504  dataFloat[2], // cx
3505  dataFloat[3], // cy
3506  dataFloat[4], // baseline
3507  localTransform));
3508  }
3509  else
3510  {
3511  UFATAL("Wrong format of the Data.calibration field (size=%d bytes, db version=%s)", dataSize, _version.c_str());
3512  }
3513  }
3514 
3515  (*iter)->sensorData().setCameraModels(models);
3516  (*iter)->sensorData().setStereoCameraModels(stereoModels);
3517  }
3518  rc = sqlite3_step(ppStmt);
3519  }
3520  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3521 
3522  //reset
3523  rc = sqlite3_reset(ppStmt);
3524  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3525  }
3526  // Finalize (delete) the statement
3527  rc = sqlite3_finalize(ppStmt);
3528  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3529 
3530  ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
3531  }
3532 
3533  // load global descriptors
3534  if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
3535  {
3536  std::stringstream query3;
3537  query3 << "SELECT type, info, data "
3538  "FROM GlobalDescriptor "
3539  "WHERE node_id = ? ";
3540 
3541  rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
3542  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3543 
3544  for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
3545  {
3546  // bind id
3547  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
3548  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3549 
3550  std::vector<GlobalDescriptor> globalDescriptors;
3551 
3552  rc = sqlite3_step(ppStmt);
3553  while(rc == SQLITE_ROW)
3554  {
3555  int index=0;
3556  const void * data = 0;
3557  int dataSize = 0;
3558  int type = -1;
3559  cv::Mat info;
3560  cv::Mat dataMat;
3561 
3562  type = sqlite3_column_int(ppStmt, index++);
3563  data = sqlite3_column_blob(ppStmt, index);
3564  dataSize = sqlite3_column_bytes(ppStmt, index++);
3565  if(dataSize && data)
3566  {
3567  info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
3568  }
3569  data = sqlite3_column_blob(ppStmt, index);
3570  dataSize = sqlite3_column_bytes(ppStmt, index++);
3571  if(dataSize && data)
3572  {
3573  dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
3574  }
3575 
3576  UASSERT(!dataMat.empty());
3577  globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
3578 
3579  rc = sqlite3_step(ppStmt);
3580  }
3581  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3582 
3583  if(!globalDescriptors.empty())
3584  {
3585  (*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
3586  ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
3587  }
3588 
3589  //reset
3590  rc = sqlite3_reset(ppStmt);
3591  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3592  }
3593  // Finalize (delete) the statement
3594  rc = sqlite3_finalize(ppStmt);
3595  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3596 
3597  ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
3598  }
3599 
3600  if(ids.size() != loaded)
3601  {
3602  UERROR("Some signatures not found in database");
3603  }
3604  }
3605 }
3606 
3607 void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
3608 {
3609  ULOGGER_DEBUG("");
3610  if(_ppDb)
3611  {
3612  std::string type;
3613  UTimer timer;
3614  timer.start();
3615  int rc = SQLITE_OK;
3616  sqlite3_stmt * ppStmt = 0;
3617  std::string query;
3618  std::list<int> ids;
3619 
3620  if(uStrNumCmp(_version, "0.11.11") >= 0)
3621  {
3622  query = "SELECT n.id "
3623  "FROM Node AS n "
3624  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
3625  "ORDER BY n.id;";
3626  }
3627  else
3628  {
3629  query = "SELECT n.id "
3630  "FROM Node AS n "
3631  "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
3632  "ORDER BY n.id;";
3633  }
3634 
3635  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
3636  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3637 
3638  // Process the result if one
3639  rc = sqlite3_step(ppStmt);
3640  while(rc == SQLITE_ROW)
3641  {
3642  ids.push_back(sqlite3_column_int(ppStmt, 0)); // Signature id
3643  rc = sqlite3_step(ppStmt); // next result...
3644  }
3645 
3646  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3647 
3648  // Finalize (delete) the statement
3649  rc = sqlite3_finalize(ppStmt);
3650  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3651 
3652  ULOGGER_DEBUG("Loading %d signatures...", ids.size());
3653  this->loadSignaturesQuery(ids, nodes);
3654  ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
3655  }
3656 }
3657 
3658 void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
3659 {
3660  ULOGGER_DEBUG("");
3661  if(_ppDb && dictionary)
3662  {
3663  std::string type;
3664  UTimer timer;
3665  timer.start();
3666  int rc = SQLITE_OK;
3667  sqlite3_stmt * ppStmt = 0;
3668  std::stringstream query;
3669  std::list<VisualWord *> visualWords;
3670 
3671  // Get the visual words
3672  query << "SELECT id, descriptor_size, descriptor FROM Word ";
3673  if(lastStateOnly)
3674  {
3675  if(uStrNumCmp(_version, "0.11.11") >= 0)
3676  {
3677  query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
3678  }
3679  else
3680  {
3681  query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
3682  }
3683  }
3684  query << "ORDER BY id;";
3685 
3686  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3687  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3688 
3689  // Process the result if one
3690  int id = 0;
3691  int descriptorSize = 0;
3692  const void * descriptor = 0;
3693  int dRealSize = 0;
3694  rc = sqlite3_step(ppStmt);
3695  int count = 0;
3696  while(rc == SQLITE_ROW)
3697  {
3698  int index=0;
3699  id = sqlite3_column_int(ppStmt, index++); // VisualWord Id
3700 
3701  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3702  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3703  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3704 
3705  cv::Mat d;
3706  if(dRealSize == descriptorSize)
3707  {
3708  // CV_8U binary descriptors
3709  d = cv::Mat(1, descriptorSize, CV_8U);
3710  }
3711  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3712  {
3713  // CV_32F
3714  d = cv::Mat(1, descriptorSize, CV_32F);
3715  }
3716  else
3717  {
3718  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3719  }
3720 
3721  memcpy(d.data, descriptor, dRealSize);
3722  VisualWord * vw = new VisualWord(id, d);
3723  vw->setSaved(true);
3724  dictionary->addWord(vw);
3725 
3726  if(++count % 5000 == 0)
3727  {
3728  ULOGGER_DEBUG("Loaded %d words...", count);
3729  }
3730  rc = sqlite3_step(ppStmt); // next result...
3731  }
3732  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3733  // Finalize (delete) the statement
3734  rc = sqlite3_finalize(ppStmt);
3735  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3736 
3737  // Get Last word id
3738  getLastWordId(id);
3739  dictionary->setLastWordId(id);
3740 
3741  ULOGGER_DEBUG("Time=%fs", timer.ticks());
3742  }
3743 }
3744 
3745 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
3746 void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
3747 {
3748  ULOGGER_DEBUG("size=%d", wordIds.size());
3749  if(_ppDb && wordIds.size())
3750  {
3751  std::string type;
3752  UTimer timer;
3753  timer.start();
3754  int rc = SQLITE_OK;
3755  sqlite3_stmt * ppStmt = 0;
3756  std::stringstream query;
3757  std::set<int> loaded;
3758 
3759  // Get the map from signature and visual words
3760  query << "SELECT vw.descriptor_size, vw.descriptor "
3761  "FROM Word as vw "
3762  "WHERE vw.id = ?;";
3763 
3764  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3765  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3766 
3767  int descriptorSize;
3768  const void * descriptor;
3769  int dRealSize;
3770  unsigned long dRealSizeTotal = 0;
3771  for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
3772  {
3773  // bind id
3774  rc = sqlite3_bind_int(ppStmt, 1, *iter);
3775  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3776 
3777  // Process the result if one
3778  rc = sqlite3_step(ppStmt);
3779  if(rc == SQLITE_ROW)
3780  {
3781  int index = 0;
3782  descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
3783  descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
3784  dRealSize = sqlite3_column_bytes(ppStmt, index++);
3785 
3786  cv::Mat d;
3787  if(dRealSize == descriptorSize)
3788  {
3789  // CV_8U binary descriptors
3790  d = cv::Mat(1, descriptorSize, CV_8U);
3791  }
3792  else if(dRealSize/int(sizeof(float)) == descriptorSize)
3793  {
3794  // CV_32F
3795  d = cv::Mat(1, descriptorSize, CV_32F);
3796  }
3797  else
3798  {
3799  UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
3800  }
3801 
3802  memcpy(d.data, descriptor, dRealSize);
3803  dRealSizeTotal+=dRealSize;
3804  VisualWord * vw = new VisualWord(*iter, d);
3805  if(vw)
3806  {
3807  vw->setSaved(true);
3808  }
3809  vws.push_back(vw);
3810  loaded.insert(loaded.end(), *iter);
3811 
3812  rc = sqlite3_step(ppStmt);
3813  }
3814 
3815  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3816 
3817  rc = sqlite3_reset(ppStmt);
3818  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3819  }
3820 
3821  // Finalize (delete) the statement
3822  rc = sqlite3_finalize(ppStmt);
3823  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3824 
3825  UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);
3826 
3827  if(wordIds.size() != loaded.size())
3828  {
3829  for(std::set<int>::const_iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
3830  {
3831  if(loaded.find(*iter) == loaded.end())
3832  {
3833  UDEBUG("Not found word %d", *iter);
3834  }
3835  }
3836  UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
3837  }
3838  }
3839 }
3840 
3841 void DBDriverSqlite3::loadLinksQuery(
3842  int signatureId,
3843  std::multimap<int, Link> & links,
3844  Link::Type typeIn) const
3845 {
3846  links.clear();
3847  if(_ppDb)
3848  {
3849  UTimer timer;
3850  timer.start();
3851  int rc = SQLITE_OK;
3852  sqlite3_stmt * ppStmt = 0;
3853  std::stringstream query;
3854 
3855  if(uStrNumCmp(_version, "0.13.0") >= 0)
3856  {
3857  query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
3858  }
3859  else if(uStrNumCmp(_version, "0.10.10") >= 0)
3860  {
3861  query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
3862  }
3863  else if(uStrNumCmp(_version, "0.8.4") >= 0)
3864  {
3865  query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
3866  }
3867  else if(uStrNumCmp(_version, "0.7.4") >= 0)
3868  {
3869  query << "SELECT to_id, type, transform, variance FROM Link ";
3870  }
3871  else
3872  {
3873  query << "SELECT to_id, type, transform FROM Link ";
3874  }
3875  query << "WHERE from_id = " << signatureId;
3876  if(typeIn < Link::kEnd)
3877  {
3878  if(uStrNumCmp(_version, "0.7.4") >= 0)
3879  {
3880  query << " AND type = " << typeIn;
3881  }
3882  else if(typeIn == Link::kNeighbor)
3883  {
3884  query << " AND type = 0";
3885  }
3886  else if(typeIn > Link::kNeighbor)
3887  {
3888  query << " AND type > 0";
3889  }
3890  }
3891  if(uStrNumCmp(_version, "0.18.3") >= 0 && (typeIn != Link::kAllWithLandmarks && typeIn != Link::kLandmark))
3892  {
3893  query << " AND type != " << Link::kLandmark;
3894  }
3895 
3896  query << " ORDER BY to_id";
3897 
3898  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
3899  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3900 
3901  int toId = -1;
3902  int type = Link::kUndef;
3903  const void * data = 0;
3904  int dataSize = 0;
3905 
3906  // Process the result if one
3907  rc = sqlite3_step(ppStmt);
3908  while(rc == SQLITE_ROW)
3909  {
3910  int index = 0;
3911 
3912  toId = sqlite3_column_int(ppStmt, index++);
3913  type = sqlite3_column_int(ppStmt, index++);
3914 
3915  data = sqlite3_column_blob(ppStmt, index);
3916  dataSize = sqlite3_column_bytes(ppStmt, index++);
3917 
3918  Transform transform;
3919  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
3920  {
3921  memcpy(transform.data(), data, dataSize);
3922  if(uStrNumCmp(_version, "0.15.2") < 0)
3923  {
3924  transform.normalizeRotation();
3925  }
3926  }
3927  else if(dataSize)
3928  {
3929  UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
3930  }
3931 
3932  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
3933  if(uStrNumCmp(_version, "0.8.4") >= 0)
3934  {
3935  if(uStrNumCmp(_version, "0.13.0") >= 0)
3936  {
3937  data = sqlite3_column_blob(ppStmt, index);
3938  dataSize = sqlite3_column_bytes(ppStmt, index++);
3939  UASSERT(dataSize==36*sizeof(double) && data);
3940  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
3941  }
3942  else
3943  {
3944  double rotVariance = sqlite3_column_double(ppStmt, index++);
3945  double transVariance = sqlite3_column_double(ppStmt, index++);
3946  UASSERT(rotVariance > 0.0 && transVariance>0.0);
3947  informationMatrix.at<double>(0,0) = 1.0/transVariance;
3948  informationMatrix.at<double>(1,1) = 1.0/transVariance;
3949  informationMatrix.at<double>(2,2) = 1.0/transVariance;
3950  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
3951  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
3952  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
3953  }
3954 
3955  cv::Mat userDataCompressed;
3956  if(uStrNumCmp(_version, "0.10.10") >= 0)
3957  {
3958  const void * data = sqlite3_column_blob(ppStmt, index);
3959  dataSize = sqlite3_column_bytes(ppStmt, index++);
3960  //Create the userData
3961  if(dataSize>4 && data)
3962  {
3963  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
3964  }
3965  }
3966 
3967  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
3968  }
3969  else if(uStrNumCmp(_version, "0.7.4") >= 0)
3970  {
3971  double variance = sqlite3_column_double(ppStmt, index++);
3972  UASSERT(variance>0.0);
3973  informationMatrix *= 1.0/variance;
3974  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
3975  }
3976  else
3977  {
3978  // neighbor is 0, loop closures are 1
3979  links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
3980  }
3981 
3982  rc = sqlite3_step(ppStmt);
3983  }
3984 
3985  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3986 
3987  // Finalize (delete) the statement
3988  rc = sqlite3_finalize(ppStmt);
3989  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
3990 
3991  if(links.size() == 0)
3992  {
3993  //UERROR("No links loaded from signature %d", signatureId);
3994  }
3995  }
3996 }
3997 
3998 void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
3999 {
4000  if(_ppDb)
4001  {
4002  UTimer timer;
4003  timer.start();
4004  int rc = SQLITE_OK;
4005  sqlite3_stmt * ppStmt = 0;
4006  std::stringstream query;
4007 
4008  if(uStrNumCmp(_version, "0.13.0") >= 0)
4009  {
4010  query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
4011  << "WHERE from_id = ? "
4012  << "ORDER BY to_id";
4013  }
4014  else if(uStrNumCmp(_version, "0.10.10") >= 0)
4015  {
4016  query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
4017  << "WHERE from_id = ? "
4018  << "ORDER BY to_id";
4019  }
4020  else if(uStrNumCmp(_version, "0.8.4") >= 0)
4021  {
4022  query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
4023  << "WHERE from_id = ? "
4024  << "ORDER BY to_id";
4025  }
4026  else if(uStrNumCmp(_version, "0.7.4") >= 0)
4027  {
4028  query << "SELECT to_id, type, variance, transform FROM Link "
4029  << "WHERE from_id = ? "
4030  << "ORDER BY to_id";
4031  }
4032  else
4033  {
4034  query << "SELECT to_id, type, transform FROM Link "
4035  << "WHERE from_id = ? "
4036  << "ORDER BY to_id";
4037  }
4038 
4039  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
4040  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4041 
4042  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
4043  {
4044  // bind id
4045  rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
4046  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4047 
4048  int toId = -1;
4049  int linkType = -1;
4050  std::list<Link> links;
4051  const void * data = 0;
4052  int dataSize = 0;
4053 
4054  // Process the result if one
4055  rc = sqlite3_step(ppStmt);
4056  while(rc == SQLITE_ROW)
4057  {
4058  int index = 0;
4059 
4060  toId = sqlite3_column_int(ppStmt, index++);
4061  linkType = sqlite3_column_int(ppStmt, index++);
4062  cv::Mat userDataCompressed;
4063  cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
4064  if(uStrNumCmp(_version, "0.8.4") >= 0)
4065  {
4066  if(uStrNumCmp(_version, "0.13.0") >= 0)
4067  {
4068  data = sqlite3_column_blob(ppStmt, index);
4069  dataSize = sqlite3_column_bytes(ppStmt, index++);
4070  UASSERT(dataSize==36*sizeof(double) && data);
4071  informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
4072  }
4073  else
4074  {
4075  double rotVariance = sqlite3_column_double(ppStmt, index++);
4076  double transVariance = sqlite3_column_double(ppStmt, index++);
4077  UASSERT(rotVariance > 0.0 && transVariance>0.0);
4078  informationMatrix.at<double>(0,0) = 1.0/transVariance;
4079  informationMatrix.at<double>(1,1) = 1.0/transVariance;
4080  informationMatrix.at<double>(2,2) = 1.0/transVariance;
4081  informationMatrix.at<double>(3,3) = 1.0/rotVariance;
4082  informationMatrix.at<double>(4,4) = 1.0/rotVariance;
4083  informationMatrix.at<double>(5,5) = 1.0/rotVariance;
4084  }
4085 
4086  if(uStrNumCmp(_version, "0.10.10") >= 0)
4087  {
4088  const void * data = sqlite3_column_blob(ppStmt, index);
4089  dataSize = sqlite3_column_bytes(ppStmt, index++);
4090  //Create the userData
4091  if(dataSize>4 && data)
4092  {
4093  userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
4094  }
4095  }
4096  }
4097  else if(uStrNumCmp(_version, "0.7.4") >= 0)
4098  {
4099  double variance = sqlite3_column_double(ppStmt, index++);
4100  UASSERT(variance>0.0);
4101  informationMatrix *= 1.0/variance;
4102  }
4103 
4104  //transform
4105  data = sqlite3_column_blob(ppStmt, index);
4106  dataSize = sqlite3_column_bytes(ppStmt, index++);
4107  Transform transform;
4108  if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
4109  {
4110  memcpy(transform.data(), data, dataSize);
4111  if(uStrNumCmp(_version, "0.15.2") < 0)
4112  {
4113  transform.normalizeRotation();
4114  }
4115  }
4116  else if(dataSize)
4117  {
4118  UERROR("Error while loading link transform from %d to %d! Setting to null...", (*iter)->id(), toId);
4119  }
4120 
4121  if(linkType >= 0 && linkType != Link::kUndef)
4122  {
4123  if(linkType == Link::kLandmark)
4124  {
4125  (*iter)->addLandmark(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
4126  }
4127  else
4128  {
4129  if(uStrNumCmp(_version, "0.7.4") >= 0)
4130  {
4131  links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
4132  }
4133  else // neighbor is 0, loop closures are 1 and 2 (child)
4134  {
4135  links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
4136  }
4137  }
4138  }
4139  else
4140  {
4141  UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
4142  linkType, (*iter)->id(), toId);
4143  }
4144 
4145  rc = sqlite3_step(ppStmt);
4146  }
4147  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4148 
4149  // add links
4150  (*iter)->addLinks(links);
4151 
4152  //reset
4153  rc = sqlite3_reset(ppStmt);
4154  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4155  UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
4156  }
4157 
4158  // Finalize (delete) the statement
4159  rc = sqlite3_finalize(ppStmt);
4160  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4161  }
4162 }
4163 
4164 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
4165 {
4166  UDEBUG("nodes = %d", nodes.size());
4167  if(_ppDb && nodes.size())
4168  {
4169  UTimer timer;
4170  timer.start();
4171  int rc = SQLITE_OK;
4172  sqlite3_stmt * ppStmt = 0;
4173  Signature * s = 0;
4174 
4175  std::string query;
4176  if(uStrNumCmp(_version, "0.8.5") >= 0)
4177  {
4178  if(updateTimestamp)
4179  {
4180  query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
4181  }
4182  else
4183  {
4184  query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
4185  }
4186  }
4187  else
4188  {
4189  if(updateTimestamp)
4190  {
4191  query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
4192  }
4193  else
4194  {
4195  query = "UPDATE Node SET weight=? WHERE id=?;";
4196  }
4197  }
4198  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4199  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4200 
4201  for(std::list<Signature *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
4202  {
4203  s = *i;
4204  int index = 1;
4205  if(s)
4206  {
4207  rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
4208  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4209 
4210  if(uStrNumCmp(_version, "0.8.5") >= 0)
4211  {
4212  if(s->getLabel().empty())
4213  {
4214  rc = sqlite3_bind_null(ppStmt, index++);
4215  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4216  }
4217  else
4218  {
4219  rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
4220  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4221  }
4222  }
4223 
4224  rc = sqlite3_bind_int(ppStmt, index++, s->id());
4225  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4226 
4227  //step
4228  rc=sqlite3_step(ppStmt);
4229  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4230 
4231  rc = sqlite3_reset(ppStmt);
4232  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4233  }
4234  }
4235  // Finalize (delete) the statement
4236  rc = sqlite3_finalize(ppStmt);
4237  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4238 
4239  ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
4240 
4241  // Update links part1
4242  if(uStrNumCmp(_version, "0.18.3") >= 0)
4243  {
4244  query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
4245  }
4246  else
4247  {
4248  query = uFormat("DELETE FROM Link WHERE from_id=?;");
4249  }
4250  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4251  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4252  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4253  {
4254  if((*j)->isLinksModified())
4255  {
4256  rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
4257  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4258 
4259  rc=sqlite3_step(ppStmt);
4260  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4261 
4262  rc = sqlite3_reset(ppStmt);
4263  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4264  }
4265  }
4266  // Finalize (delete) the statement
4267  rc = sqlite3_finalize(ppStmt);
4268  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4269 
4270  // Update links part2
4271  query = queryStepLink();
4272  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4273  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4274  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4275  {
4276  if((*j)->isLinksModified())
4277  {
4278  // Save links
4279  const std::multimap<int, Link> & links = (*j)->getLinks();
4280  for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4281  {
4282  stepLink(ppStmt, i->second);
4283  }
4284  }
4285  }
4286  // Finalize (delete) the statement
4287  rc = sqlite3_finalize(ppStmt);
4288  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4289  ULOGGER_DEBUG("Update Neighbors Time=%fs", timer.ticks());
4290 
4291  // Update word references
4292  query = queryStepWordsChanged();
4293  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4294  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4295  for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
4296  {
4297  if((*j)->getWordsChanged().size())
4298  {
4299  const std::map<int, int> & wordsChanged = (*j)->getWordsChanged();
4300  for(std::map<int, int>::const_iterator iter=wordsChanged.begin(); iter!=wordsChanged.end(); ++iter)
4301  {
4302  stepWordsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
4303  }
4304  }
4305  }
4306  // Finalize (delete) the statement
4307  rc = sqlite3_finalize(ppStmt);
4308  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4309 
4310  ULOGGER_DEBUG("signatures update=%fs", timer.ticks());
4311  }
4312 }
4313 
4314 void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
4315 {
4316  if(_ppDb && words.size() && updateTimestamp)
4317  {
4318  // Only timestamp update is done here, so don't enter this if at all if false
4319  UTimer timer;
4320  timer.start();
4321  int rc = SQLITE_OK;
4322  sqlite3_stmt * ppStmt = 0;
4323  VisualWord * w = 0;
4324 
4325  std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
4326  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4327  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4328 
4329  for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
4330  {
4331  w = *i;
4332  int index = 1;
4333  UASSERT(w);
4334 
4335  rc = sqlite3_bind_int(ppStmt, index++, w->id());
4336  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4337 
4338  //step
4339  rc=sqlite3_step(ppStmt);
4340  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4341 
4342  rc = sqlite3_reset(ppStmt);
4343  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4344 
4345  }
4346  // Finalize (delete) the statement
4347  rc = sqlite3_finalize(ppStmt);
4348  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4349 
4350  ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
4351  }
4352 }
4353 
4354 void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
4355 {
4356  UDEBUG("");
4357  if(_ppDb && signatures.size())
4358  {
4359  std::string type;
4360  UTimer timer;
4361  timer.start();
4362  int rc = SQLITE_OK;
4363  sqlite3_stmt * ppStmt = 0;
4364 
4365  // Signature table
4366  std::string query = queryStepNode();
4367  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4368  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4369 
4370  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4371  {
4372  _memoryUsedEstimate += (*i)->getMemoryUsed();
4373  // raw data are not kept in database
4374  _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
4375  _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
4376  _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
4377 
4378  stepNode(ppStmt, *i);
4379  }
4380  // Finalize (delete) the statement
4381  rc = sqlite3_finalize(ppStmt);
4382  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4383 
4384  UDEBUG("Time=%fs", timer.ticks());
4385 
4386  // Create new entries in table Link
4387  query = queryStepLink();
4388  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4389  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4390  for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
4391  {
4392  // Save links
4393  const std::multimap<int, Link> & links = (*jter)->getLinks();
4394  for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4395  {
4396  stepLink(ppStmt, i->second);
4397  }
4398  if(uStrNumCmp(_version, "0.18.3") >= 0)
4399  {
4400  // Save landmarks
4401  const std::map<int, Link> & links = (*jter)->getLandmarks();
4402  for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
4403  {
4404  stepLink(ppStmt, i->second);
4405  }
4406  }
4407  }
4408  // Finalize (delete) the statement
4409  rc = sqlite3_finalize(ppStmt);
4410  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4411 
4412  UDEBUG("Time=%fs", timer.ticks());
4413 
4414 
4415  // Create new entries in table Feature
4416  query = queryStepKeypoint();
4417  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4418  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4419  float nanFloat = std::numeric_limits<float>::quiet_NaN ();
4420  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4421  {
4422  UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
4423  UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
4424  UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
4425 
4426  for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
4427  {
4428  cv::Point3f pt(nanFloat,nanFloat,nanFloat);
4429  if(!(*i)->getWords3().empty())
4430  {
4431  pt = (*i)->getWords3()[w->second];
4432  }
4433 
4434  cv::Mat descriptor;
4435  if(!(*i)->getWordsDescriptors().empty())
4436  {
4437  descriptor = (*i)->getWordsDescriptors().row(w->second);
4438  }
4439 
4440  stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
4441  }
4442  }
4443  // Finalize (delete) the statement
4444  rc = sqlite3_finalize(ppStmt);
4445  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4446  UDEBUG("Time=%fs", timer.ticks());
4447 
4448  if(uStrNumCmp(_version, "0.20.0") >= 0)
4449  {
4450  // Global descriptor table
4451  std::string query = queryStepGlobalDescriptor();
4452  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4453  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4454 
4455  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4456  {
4457  for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
4458  {
4459  stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
4460  }
4461  }
4462  // Finalize (delete) the statement
4463  rc = sqlite3_finalize(ppStmt);
4464  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4465 
4466  UDEBUG("Time=%fs", timer.ticks());
4467  }
4468 
4469  if(uStrNumCmp(_version, "0.10.0") >= 0)
4470  {
4471  // Add SensorData
4472  query = queryStepSensorData();
4473  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4474  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4475  UDEBUG("Saving %d images", signatures.size());
4476 
4477  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4478  {
4479  if(!(*i)->sensorData().imageCompressed().empty() ||
4480  !(*i)->sensorData().depthOrRightCompressed().empty() ||
4481  !(*i)->sensorData().laserScanCompressed().isEmpty() ||
4482  !(*i)->sensorData().userDataCompressed().empty() ||
4483  !(*i)->sensorData().cameraModels().empty() ||
4484  !(*i)->sensorData().stereoCameraModels().empty())
4485  {
4486  UASSERT((*i)->id() == (*i)->sensorData().id());
4487  stepSensorData(ppStmt, (*i)->sensorData());
4488  }
4489  }
4490 
4491  // Finalize (delete) the statement
4492  rc = sqlite3_finalize(ppStmt);
4493  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4494  UDEBUG("Time=%fs", timer.ticks());
4495  }
4496  else
4497  {
4498  // Add images
4499  query = queryStepImage();
4500  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4501  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4502  UDEBUG("Saving %d images", signatures.size());
4503 
4504  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4505  {
4506  if(!(*i)->sensorData().imageCompressed().empty())
4507  {
4508  stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
4509  }
4510  }
4511 
4512  // Finalize (delete) the statement
4513  rc = sqlite3_finalize(ppStmt);
4514  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4515  UDEBUG("Time=%fs", timer.ticks());
4516 
4517  // Add depths
4518  query = queryStepDepth();
4519  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4520  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4521  for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
4522  {
4523  //metric
4524  if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().isEmpty())
4525  {
4526  UASSERT((*i)->id() == (*i)->sensorData().id());
4527  stepDepth(ppStmt, (*i)->sensorData());
4528  }
4529  }
4530  // Finalize (delete) the statement
4531  rc = sqlite3_finalize(ppStmt);
4532  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4533  }
4534 
4535  UDEBUG("Time=%fs", timer.ticks());
4536  }
4537 }
4538 
4539 void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
4540 {
4541  UDEBUG("visualWords size=%d", words.size());
4542  if(_ppDb)
4543  {
4544  std::string type;
4545  UTimer timer;
4546  timer.start();
4547  int rc = SQLITE_OK;
4548  sqlite3_stmt * ppStmt = 0;
4549  std::string query;
4550 
4551  // Create new entries in table Map_SS_VW
4552  if(words.size()>0)
4553  {
4554  query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
4555  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4556  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4557  for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
4558  {
4559  const VisualWord * w = *iter;
4560  UASSERT(w);
4561  if(!w->isSaved())
4562  {
4563  rc = sqlite3_bind_int(ppStmt, 1, w->id());
4564  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4565  rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
4566  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4567  UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
4568  if(w->getDescriptor().type() == CV_32F)
4569  {
4570  // CV_32F
4571  rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
4572  }
4573  else
4574  {
4575  // CV_8U
4576  rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
4577  }
4578  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4579 
4580  //execute query
4581  rc=sqlite3_step(ppStmt);
4582  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
4583 
4584  rc = sqlite3_reset(ppStmt);
4585  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4586  }
4587  }
4588  // Finalize (delete) the statement
4589  rc = sqlite3_finalize(ppStmt);
4590  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4591  }
4592 
4593  UDEBUG("Time=%fs", timer.ticks());
4594  }
4595 }
4596 
4597 void DBDriverSqlite3::addLinkQuery(const Link & link) const
4598 {
4599  UDEBUG("");
4600  if(_ppDb)
4601  {
4602  std::string type;
4603  UTimer timer;
4604  timer.start();
4605  int rc = SQLITE_OK;
4606  sqlite3_stmt * ppStmt = 0;
4607 
4608  // Create new entries in table Link
4609  std::string query = queryStepLink();
4610  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4611  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4612 
4613  // Save link
4614  stepLink(ppStmt, link);
4615 
4616  // Finalize (delete) the statement
4617  rc = sqlite3_finalize(ppStmt);
4618  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4619 
4620  UDEBUG("Time=%fs", timer.ticks());
4621  }
4622 
4623 }
4624 
4625 void DBDriverSqlite3::updateLinkQuery(const Link & link) const
4626 {
4627  UDEBUG("");
4628  if(_ppDb)
4629  {
4630  std::string type;
4631  UTimer timer;
4632  timer.start();
4633  int rc = SQLITE_OK;
4634  sqlite3_stmt * ppStmt = 0;
4635 
4636  // Create new entries in table Link
4637  std::string query = queryStepLinkUpdate();
4638  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4639  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4640 
4641  // Save link
4642  stepLink(ppStmt, link);
4643 
4644  // Finalize (delete) the statement
4645  rc = sqlite3_finalize(ppStmt);
4646  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4647 
4648  UDEBUG("Time=%fs", timer.ticks());
4649  }
4650 }
4651 
4652 void DBDriverSqlite3::updateOccupancyGridQuery(
4653  int nodeId,
4654  const cv::Mat & ground,
4655  const cv::Mat & obstacles,
4656  const cv::Mat & empty,
4657  float cellSize,
4658  const cv::Point3f & viewpoint) const
4659 {
4660  UDEBUG("");
4661  if(_ppDb)
4662  {
4663  std::string type;
4664  UTimer timer;
4665  timer.start();
4666  int rc = SQLITE_OK;
4667  sqlite3_stmt * ppStmt = 0;
4668 
4669  // Create query
4670  std::string query = queryStepOccupancyGridUpdate();
4671  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4672  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4673 
4674  // Save occupancy grid
4675  stepOccupancyGridUpdate(ppStmt,
4676  nodeId,
4677  ground,
4678  obstacles,
4679  empty,
4680  cellSize,
4681  viewpoint);
4682 
4683  // Finalize (delete) the statement
4684  rc = sqlite3_finalize(ppStmt);
4685  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4686 
4687  UDEBUG("Time=%fs", timer.ticks());
4688  }
4689 }
4690 
4691 void DBDriverSqlite3::updateCalibrationQuery(
4692  int nodeId,
4693  const std::vector<CameraModel> & models,
4694  const std::vector<StereoCameraModel> & stereoModels) const
4695 {
4696  UDEBUG("");
4697  if(_ppDb)
4698  {
4699  std::string type;
4700  UTimer timer;
4701  timer.start();
4702  int rc = SQLITE_OK;
4703  sqlite3_stmt * ppStmt = 0;
4704 
4705  // Create query
4706  std::string query = queryStepCalibrationUpdate();
4707  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4708  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4709 
4710  // step calibration
4711  stepCalibrationUpdate(ppStmt,
4712  nodeId,
4713  models,
4714  stereoModels);
4715 
4716  // Finalize (delete) the statement
4717  rc = sqlite3_finalize(ppStmt);
4718  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4719 
4720  UDEBUG("Time=%fs", timer.ticks());
4721  }
4722 }
4723 
4724 void DBDriverSqlite3::updateDepthImageQuery(
4725  int nodeId,
4726  const cv::Mat & image,
4727  const std::string & format) const
4728 {
4729  UDEBUG("");
4730  if(_ppDb)
4731  {
4732  std::string type;
4733  UTimer timer;
4734  timer.start();
4735  int rc = SQLITE_OK;
4736  sqlite3_stmt * ppStmt = 0;
4737 
4738  // Create query
4739  std::string query = queryStepDepthUpdate();
4740  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4741  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4742 
4743  // Save depth
4744  stepDepthUpdate(ppStmt,
4745  nodeId,
4746  image,
4747  format);
4748 
4749  // Finalize (delete) the statement
4750  rc = sqlite3_finalize(ppStmt);
4751  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4752 
4753  UDEBUG("Time=%fs", timer.ticks());
4754  }
4755 }
4756 
4757 void DBDriverSqlite3::updateLaserScanQuery(
4758  int nodeId,
4759  const LaserScan & scan) const
4760 {
4761  UDEBUG("");
4762  if(_ppDb)
4763  {
4764  std::string type;
4765  UTimer timer;
4766  timer.start();
4767  int rc = SQLITE_OK;
4768  sqlite3_stmt * ppStmt = 0;
4769 
4770  // Create query
4771  std::string query = queryStepScanUpdate();
4772  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4773  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4774 
4775  // Save depth
4776  stepScanUpdate(ppStmt,
4777  nodeId,
4778  scan);
4779 
4780  // Finalize (delete) the statement
4781  rc = sqlite3_finalize(ppStmt);
4782  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4783 
4784  UDEBUG("Time=%fs", timer.ticks());
4785  }
4786 }
4787 
4788 void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
4789 {
4790  UDEBUG("Ref ID = %d", statistics.refImageId());
4791  if(_ppDb)
4792  {
4793  std::string type;
4794  UTimer timer;
4795  timer.start();
4796  int rc = SQLITE_OK;
4797  sqlite3_stmt * ppStmt = 0;
4798 
4799  // Create query
4800  if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
4801  {
4802  std::string param = Statistics::serializeData(statistics.data());
4803  if(param.size() && statistics.refImageId()>0)
4804  {
4805  std::string query;
4806  if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
4807  {
4808  query = "INSERT INTO Statistics(id, stamp, data, wm_state) values(?,?,?,?);";
4809  }
4810  else
4811  {
4812  query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
4813  }
4814  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4815  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4816 
4817  int index = 1;
4818  rc = sqlite3_bind_int(ppStmt, index++, statistics.refImageId());
4819  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4820  rc = sqlite3_bind_double(ppStmt, index++, statistics.stamp());
4821  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4822 
4823  cv::Mat compressedParam;
4824  if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
4825  {
4826  compressedParam = compressString(param);
4827  rc = sqlite3_bind_blob(ppStmt, index++, compressedParam.data, compressedParam.cols, SQLITE_STATIC);
4828  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4829  }
4830  else
4831  {
4832  rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, SQLITE_STATIC);
4833  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4834  }
4835 
4836  cv::Mat compressedWmState;
4837  if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
4838  {
4839  if(saveWmState && !statistics.wmState().empty())
4840  {
4841  compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
4842  rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
4843  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4844  }
4845  else
4846  {
4847  rc = sqlite3_bind_null(ppStmt, index++);
4848  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4849  }
4850  }
4851 
4852  //step
4853  rc=sqlite3_step(ppStmt);
4854  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4855 
4856  rc = sqlite3_reset(ppStmt);
4857  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4858 
4859  // Finalize (delete) the statement
4860  rc = sqlite3_finalize(ppStmt);
4861  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4862 
4863  UDEBUG("Time=%fs", timer.ticks());
4864  }
4865  }
4866  }
4867 }
4868 
4869 void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
4870 {
4871  UDEBUG("");
4872  if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
4873  {
4874  UTimer timer;
4875  timer.start();
4876  int rc = SQLITE_OK;
4877  sqlite3_stmt * ppStmt = 0;
4878  std::string query;
4879 
4880  // Update table Admin
4881  query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
4882  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4883  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4884 
4885  int index = 1;
4886  cv::Mat compressedImage;
4887  if(image.empty())
4888  {
4889  rc = sqlite3_bind_null(ppStmt, index);
4890  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4891  }
4892  else
4893  {
4894  // compress
4895  if(image.rows == 1 && image.type() == CV_8UC1)
4896  {
4897  // already compressed
4898  compressedImage = image;
4899  }
4900  else
4901  {
4902  compressedImage = compressImage2(image, ".jpg");
4903  }
4904  rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
4905  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4906  }
4907 
4908  //execute query
4909  rc=sqlite3_step(ppStmt);
4910  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4911 
4912  // Finalize (delete) the statement
4913  rc = sqlite3_finalize(ppStmt);
4914  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4915 
4916  UDEBUG("Time=%fs", timer.ticks());
4917  }
4918 }
4919 cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
4920 {
4921  UDEBUG("");
4922  cv::Mat image;
4923  if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
4924  {
4925  UTimer timer;
4926  timer.start();
4927  int rc = SQLITE_OK;
4928  sqlite3_stmt * ppStmt = 0;
4929  std::stringstream query;
4930 
4931  query << "SELECT preview_image "
4932  << "FROM Admin "
4933  << "WHERE version='" << _version.c_str()
4934  <<"';";
4935 
4936  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
4937  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4938 
4939  // Process the result if one
4940  rc = sqlite3_step(ppStmt);
4941  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());
4942  if(rc == SQLITE_ROW)
4943  {
4944  const void * data = 0;
4945  int dataSize = 0;
4946  int index = 0;
4947 
4948  //opt_cloud
4949  data = sqlite3_column_blob(ppStmt, index);
4950  dataSize = sqlite3_column_bytes(ppStmt, index++);
4951  if(dataSize>0 && data)
4952  {
4953  image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
4954  }
4955  UDEBUG("Image=%dx%d", image.cols, image.rows);
4956 
4957  rc = sqlite3_step(ppStmt); // next result...
4958  }
4959  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4960 
4961  // Finalize (delete) the statement
4962  rc = sqlite3_finalize(ppStmt);
4963  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4964  ULOGGER_DEBUG("Time=%fs", timer.ticks());
4965 
4966  }
4967  return image;
4968 }
4969 
4970 void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
4971 {
4972  UDEBUG("poses=%d lastlocalizationPose=%s", (int)poses.size(), lastlocalizationPose.prettyPrint().c_str());
4973  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
4974  {
4975  UTimer timer;
4976  timer.start();
4977  int rc = SQLITE_OK;
4978  sqlite3_stmt * ppStmt = 0;
4979  std::string query;
4980 
4981  // Update table Admin
4982  query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
4983  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
4984  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4985 
4986  int index = 1;
4987 
4988  // opt ids and poses
4989  cv::Mat compressedIds;
4990  cv::Mat compressedPoses;
4991  if(poses.empty())
4992  {
4993  rc = sqlite3_bind_null(ppStmt, index++);
4994  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4995  rc = sqlite3_bind_null(ppStmt, index++);
4996  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
4997  }
4998  else
4999  {
5000  std::vector<int> serializedIds(poses.size());
5001  std::vector<float> serializedPoses(poses.size()*12);
5002  int i=0;
5003  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
5004  {
5005  serializedIds[i] = iter->first;
5006  memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
5007  ++i;
5008  }
5009 
5010  compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
5011  compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
5012 
5013  rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
5014  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5015  rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
5016  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5017  }
5018 
5019  if(lastlocalizationPose.isNull())
5020  {
5021  rc = sqlite3_bind_null(ppStmt, index++);
5022  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5023  }
5024  else
5025  {
5026  UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
5027  rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
5028  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5029  }
5030 
5031  //execute query
5032  rc=sqlite3_step(ppStmt);
5033  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5034 
5035  // Finalize (delete) the statement
5036  rc = sqlite3_finalize(ppStmt);
5037  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5038 
5039  UDEBUG("Time=%fs", timer.ticks());
5040  }
5041 }
5042 
5043 std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
5044 {
5045  UDEBUG("");
5046  std::map<int, Transform> poses;
5047  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
5048  {
5049  UTimer timer;
5050  timer.start();
5051  int rc = SQLITE_OK;
5052  sqlite3_stmt * ppStmt = 0;
5053  std::stringstream query;
5054 
5055  query << "SELECT opt_ids, opt_poses, opt_last_localization "
5056  << "FROM Admin "
5057  << "WHERE version='" << _version.c_str()
5058  <<"';";
5059 
5060  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
5061  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5062 
5063  // Process the result if one
5064  rc = sqlite3_step(ppStmt);
5065  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());
5066  if(rc == SQLITE_ROW)
5067  {
5068  const void * data = 0;
5069  int dataSize = 0;
5070  int index = 0;
5071 
5072  //opt_poses
5073  cv::Mat serializedIds;
5074  data = sqlite3_column_blob(ppStmt, index);
5075  dataSize = sqlite3_column_bytes(ppStmt, index++);
5076  if(dataSize>0 && data)
5077  {
5078  serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5079  UDEBUG("serializedIds=%d", serializedIds.cols);
5080  }
5081 
5082  data = sqlite3_column_blob(ppStmt, index);
5083  dataSize = sqlite3_column_bytes(ppStmt, index++);
5084  if(dataSize>0 && data)
5085  {
5086  cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5087  UDEBUG("serializedPoses=%d", serializedPoses.cols);
5088 
5089  UASSERT(serializedIds.cols == serializedPoses.cols/12);
5090  UASSERT(serializedPoses.type() == CV_32FC1);
5091  UASSERT(serializedIds.type() == CV_32SC1);
5092  for(int i=0; i<serializedIds.cols; ++i)
5093  {
5094  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),
5095  serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
5096  serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
5097  poses.insert(std::make_pair(serializedIds.at<int>(i), t));
5098  UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
5099  }
5100  }
5101 
5102  data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
5103  dataSize = sqlite3_column_bytes(ppStmt, index++);
5104  if(lastlocalizationPose)
5105  {
5106  if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
5107  {
5108  memcpy(lastlocalizationPose->data(), data, dataSize);
5109  }
5110  UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
5111  }
5112 
5113  rc = sqlite3_step(ppStmt); // next result...
5114  }
5115  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5116 
5117  // Finalize (delete) the statement
5118  rc = sqlite3_finalize(ppStmt);
5119  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5120  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5121 
5122  }
5123  return poses;
5124 }
5125 
5126 void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
5127 {
5128  UDEBUG("");
5129  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
5130  {
5131  UTimer timer;
5132  timer.start();
5133  int rc = SQLITE_OK;
5134  sqlite3_stmt * ppStmt = 0;
5135  std::string query;
5136 
5137  // Update table Admin
5138  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());
5139  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
5140  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5141 
5142  int index = 1;
5143 
5144  // opt ids and poses
5145  cv::Mat compressedMap;
5146  if(map.empty())
5147  {
5148  rc = sqlite3_bind_null(ppStmt, index++);
5149  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5150  }
5151  else
5152  {
5153  compressedMap = compressData2(map);
5154 
5155  rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
5156  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5157  }
5158 
5159  rc = sqlite3_bind_double(ppStmt, index++, xMin);
5160  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5161  rc = sqlite3_bind_double(ppStmt, index++, yMin);
5162  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5163  rc = sqlite3_bind_double(ppStmt, index++, cellSize);
5164  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5165 
5166  //execute query
5167  rc=sqlite3_step(ppStmt);
5168  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5169 
5170  // Finalize (delete) the statement
5171  rc = sqlite3_finalize(ppStmt);
5172  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5173 
5174  UDEBUG("Time=%fs", timer.ticks());
5175  }
5176 }
5177 
5178 cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
5179 {
5180  UDEBUG("");
5181  cv::Mat map;
5182  if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
5183  {
5184  UTimer timer;
5185  timer.start();
5186  int rc = SQLITE_OK;
5187  sqlite3_stmt * ppStmt = 0;
5188  std::stringstream query;
5189 
5190  query << "SELECT opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
5191  << "FROM Admin "
5192  << "WHERE version='" << _version.c_str()
5193  <<"';";
5194 
5195  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
5196  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5197 
5198  // Process the result if one
5199  rc = sqlite3_step(ppStmt);
5200  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());
5201  if(rc == SQLITE_ROW)
5202  {
5203  const void * data = 0;
5204  int dataSize = 0;
5205  int index = 0;
5206 
5207  //opt_map
5208  data = sqlite3_column_blob(ppStmt, index);
5209  dataSize = sqlite3_column_bytes(ppStmt, index++);
5210  if(dataSize>0 && data)
5211  {
5212  map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5213  UDEBUG("map=%d/%d", map.cols, map.rows);
5214  }
5215 
5216  xMin = sqlite3_column_double(ppStmt, index++);
5217  UDEBUG("xMin=%f", xMin);
5218  yMin = sqlite3_column_double(ppStmt, index++);
5219  UDEBUG("yMin=%f", yMin);
5220  cellSize = sqlite3_column_double(ppStmt, index++);
5221  UDEBUG("cellSize=%f", cellSize);
5222 
5223  rc = sqlite3_step(ppStmt); // next result...
5224  }
5225  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5226 
5227  // Finalize (delete) the statement
5228  rc = sqlite3_finalize(ppStmt);
5229  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5230  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5231 
5232  }
5233  return map;
5234 }
5235 
5236 void DBDriverSqlite3::saveOptimizedMeshQuery(
5237  const cv::Mat & cloud,
5238  const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
5239 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5240  const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
5241 #else
5242  const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
5243 #endif
5244  const cv::Mat & textures) const
5245 {
5246  UDEBUG("");
5247  if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
5248  {
5249  UTimer timer;
5250  timer.start();
5251  int rc = SQLITE_OK;
5252  sqlite3_stmt * ppStmt = 0;
5253  std::string query;
5254 
5255  // Update table Admin
5256  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());
5257  rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
5258  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5259 
5260  if(cloud.empty())
5261  {
5262  // set all fields to null
5263  for(int i=1; i<=5; ++i)
5264  {
5265  rc = sqlite3_bind_null(ppStmt, i);
5266  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5267  }
5268 
5269  //execute query
5270  rc=sqlite3_step(ppStmt);
5271  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5272  }
5273  else
5274  {
5275  int index = 1;
5276 
5277  // compress and save cloud
5278  cv::Mat compressedCloud;
5279  if(cloud.rows == 1 && cloud.type() == CV_8UC1)
5280  {
5281  // already compressed
5282  compressedCloud = cloud;
5283  }
5284  else
5285  {
5286  UDEBUG("Cloud points=%d", cloud.cols);
5287  compressedCloud = compressData2(cloud);
5288  }
5289  UDEBUG("Cloud compressed bytes=%d", compressedCloud.cols);
5290  rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
5291  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5292 
5293  // opt ids and poses
5294  cv::Mat compressedPolygons;
5295  cv::Mat compressedTexCoords;
5296  cv::Mat compressedTextures;
5297  // polygons
5298  if(polygons.empty())
5299  {
5300  //polygon size
5301  rc = sqlite3_bind_null(ppStmt, index++);
5302  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5303  // polygons
5304  rc = sqlite3_bind_null(ppStmt, index++);
5305  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5306  // tex_coords
5307  rc = sqlite3_bind_null(ppStmt, index++);
5308  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5309  // materials
5310  rc = sqlite3_bind_null(ppStmt, index++);
5311  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5312  }
5313  else
5314  {
5315  std::vector<int> serializedPolygons;
5316  std::vector<float> serializedTexCoords;
5317  int polygonSize = 0;
5318  int totalPolygonIndices = 0;
5319  UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
5320  for(unsigned int t=0; t<polygons.size(); ++t)
5321  {
5322  UDEBUG("t=%d, polygons=%d", t, (int)polygons[t].size());
5323  unsigned int materialPolygonIndices = 0;
5324  for(unsigned int p=0; p<polygons[t].size(); ++p)
5325  {
5326  if(polygonSize == 0)
5327  {
5328  UASSERT(polygons[t][p].size());
5329  polygonSize = polygons[t][p].size();
5330  }
5331  else
5332  {
5333  UASSERT(polygonSize == (int)polygons[t][p].size());
5334  }
5335 
5336  materialPolygonIndices += polygons[t][p].size();
5337  }
5338  totalPolygonIndices += materialPolygonIndices;
5339 
5340  if(!texCoords.empty())
5341  {
5342  UASSERT(materialPolygonIndices == texCoords[t].size());
5343  }
5344  }
5345  UASSERT(totalPolygonIndices>0);
5346  serializedPolygons.resize(totalPolygonIndices+polygons.size());
5347  if(!texCoords.empty())
5348  {
5349  serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
5350  }
5351 
5352  int oi=0;
5353  int ci=0;
5354  for(unsigned int t=0; t<polygons.size(); ++t)
5355  {
5356  serializedPolygons[oi++] = polygons[t].size();
5357  if(!texCoords.empty())
5358  {
5359  serializedTexCoords[ci++] = texCoords[t].size();
5360  }
5361  for(unsigned int p=0; p<polygons[t].size(); ++p)
5362  {
5363  int texIndex = p*polygonSize;
5364  for(unsigned int i=0; i<polygons[t][p].size(); ++i)
5365  {
5366  serializedPolygons[oi++] = polygons[t][p][i];
5367 
5368  if(!texCoords.empty())
5369  {
5370  serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
5371  serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
5372  }
5373  }
5374  }
5375  }
5376 
5377  UDEBUG("serializedPolygons=%d", (int)serializedPolygons.size());
5378  compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
5379 
5380  // polygon size
5381  rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
5382  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5383 
5384  rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
5385  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5386 
5387  // tex coords
5388  if(texCoords.empty())
5389  {
5390  // tex coords
5391  rc = sqlite3_bind_null(ppStmt, index++);
5392  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5393  // materials
5394  rc = sqlite3_bind_null(ppStmt, index++);
5395  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5396  }
5397  else
5398  {
5399  UDEBUG("serializedTexCoords=%d", (int)serializedTexCoords.size());
5400  compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
5401  rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
5402  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5403 
5404  UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
5405  if(textures.rows == 1 && textures.type() == CV_8UC1)
5406  {
5407  //already compressed
5408  compressedTextures = textures;
5409  }
5410  else
5411  {
5412  compressedTextures = compressImage2(textures, ".jpg");
5413  }
5414  rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
5415  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5416  }
5417  }
5418 
5419  //execute query
5420  rc=sqlite3_step(ppStmt);
5421  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5422  }
5423 
5424  // Finalize (delete) the statement
5425  rc = sqlite3_finalize(ppStmt);
5426  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5427 
5428  UDEBUG("Time=%fs", timer.ticks());
5429  }
5430 }
5431 
5432 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
5433  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
5434 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5435  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
5436 #else
5437  std::vector<std::vector<Eigen::Vector2f> > * texCoords,
5438 #endif
5439  cv::Mat * textures) const
5440 {
5441  UDEBUG("");
5442  cv::Mat cloud;
5443  if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
5444  {
5445  UTimer timer;
5446  timer.start();
5447  int rc = SQLITE_OK;
5448  sqlite3_stmt * ppStmt = 0;
5449  std::stringstream query;
5450 
5451  query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
5452  << "FROM Admin "
5453  << "WHERE version='" << _version.c_str()
5454  <<"';";
5455 
5456  rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
5457  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5458 
5459  // Process the result if one
5460  rc = sqlite3_step(ppStmt);
5461  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());
5462  if(rc == SQLITE_ROW)
5463  {
5464  const void * data = 0;
5465  int dataSize = 0;
5466  int index = 0;
5467 
5468  //opt_cloud
5469  data = sqlite3_column_blob(ppStmt, index);
5470  dataSize = sqlite3_column_bytes(ppStmt, index++);
5471  if(dataSize>0 && data)
5472  {
5473  cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5474  }
5475  UDEBUG("Cloud=%d points", cloud.cols);
5476 
5477  //opt_polygons_size
5478  int polygonSize = sqlite3_column_int(ppStmt, index++);
5479  UDEBUG("polygonSize=%d", polygonSize);
5480 
5481  //opt_polygons
5482  data = sqlite3_column_blob(ppStmt, index);
5483  dataSize = sqlite3_column_bytes(ppStmt, index++);
5484  if(dataSize>0 && data)
5485  {
5486  UASSERT(polygonSize > 0);
5487  if(polygons)
5488  {
5489  cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5490  UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
5491  UASSERT(serializedPolygons.total());
5492  for(int t=0; t<serializedPolygons.cols; ++t)
5493  {
5494  UASSERT(serializedPolygons.at<int>(t) > 0);
5495  std::vector<std::vector<RTABMAP_PCL_INDEX> > materialPolygons(serializedPolygons.at<int>(t), std::vector<RTABMAP_PCL_INDEX>(polygonSize));
5496  ++t;
5497  UASSERT(t < serializedPolygons.cols);
5498  UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
5499  for(int p=0; p<(int)materialPolygons.size(); ++p)
5500  {
5501  for(int i=0; i<polygonSize; ++i)
5502  {
5503  materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
5504  }
5505  }
5506  t+=materialPolygons.size()*polygonSize;
5507  polygons->push_back(materialPolygons);
5508  }
5509  }
5510 
5511  //opt_tex_coords
5512  data = sqlite3_column_blob(ppStmt, index);
5513  dataSize = sqlite3_column_bytes(ppStmt, index++);
5514  if(dataSize>0 && data)
5515  {
5516  if(texCoords)
5517  {
5518  cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5519  UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
5520  UASSERT(serializedTexCoords.total());
5521  for(int t=0; t<serializedTexCoords.cols; ++t)
5522  {
5523  UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
5524 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
5525  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
5526 #else
5527  std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
5528 #endif
5529  ++t;
5530  UASSERT(t < serializedTexCoords.cols);
5531  UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
5532  for(int p=0; p<(int)materialtexCoords.size(); ++p)
5533  {
5534  materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
5535  materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
5536  }
5537  t+=materialtexCoords.size()*2;
5538  texCoords->push_back(materialtexCoords);
5539  }
5540  }
5541 
5542  //opt_tex_materials
5543  data = sqlite3_column_blob(ppStmt, index);
5544  dataSize = sqlite3_column_bytes(ppStmt, index++);
5545  if(dataSize>0 && data)
5546  {
5547  if(textures)
5548  {
5549  *textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
5550  UDEBUG("textures=%dx%d", textures->cols, textures->rows);
5551  }
5552  }
5553  }
5554  }
5555 
5556  rc = sqlite3_step(ppStmt); // next result...
5557  }
5558  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5559 
5560  // Finalize (delete) the statement
5561  rc = sqlite3_finalize(ppStmt);
5562  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5563  ULOGGER_DEBUG("Time=%fs", timer.ticks());
5564 
5565  }
5566  return cloud;
5567 }
5568 
5569 std::string DBDriverSqlite3::queryStepNode() const
5570 {
5571  if(uStrNumCmp(_version, "0.18.0") >= 0)
5572  {
5573  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors) VALUES(?,?,?,?,?,?,?,?,?,?);";
5574  }
5575  else if(uStrNumCmp(_version, "0.14.0") >= 0)
5576  {
5577  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps) VALUES(?,?,?,?,?,?,?,?,?);";
5578  }
5579  else if(uStrNumCmp(_version, "0.13.0") >= 0)
5580  {
5581  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
5582  }
5583  else if(uStrNumCmp(_version, "0.11.1") >= 0)
5584  {
5585  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
5586  }
5587  else if(uStrNumCmp(_version, "0.10.1") >= 0)
5588  {
5589  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
5590  }
5591  else if(uStrNumCmp(_version, "0.8.8") >= 0)
5592  {
5593  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
5594  }
5595  else if(uStrNumCmp(_version, "0.8.5") >= 0)
5596  {
5597  return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
5598  }
5599  return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
5600 }
5601 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
5602 {
5603  UDEBUG("Save node %d", s->id());
5604  if(!ppStmt || !s)
5605  {
5606  UFATAL("");
5607  }
5608  int rc = SQLITE_OK;
5609 
5610  int index = 1;
5611  rc = sqlite3_bind_int(ppStmt, index++, s->id());
5612  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5613  rc = sqlite3_bind_int(ppStmt, index++, s->mapId());
5614  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5615  rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
5616  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5617  rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
5618  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5619 
5620  if(uStrNumCmp(_version, "0.8.5") >= 0)
5621  {
5622  rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
5623  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5624 
5625  if(s->getLabel().empty())
5626  {
5627  rc = sqlite3_bind_null(ppStmt, index++);
5628  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5629  }
5630  else
5631  {
5632  rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
5633  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5634  }
5635  }
5636 
5637  std::vector<double> gps;
5638  std::vector<double> envSensors;
5639  if(uStrNumCmp(_version, "0.10.1") >= 0)
5640  {
5641  // ignore user_data
5642 
5643  if(uStrNumCmp(_version, "0.11.1") >= 0)
5644  {
5645  rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
5646  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5647 
5648  if(uStrNumCmp(_version, "0.13.0") >= 0)
5649  {
5650  if(s->getVelocity().empty())
5651  {
5652  rc = sqlite3_bind_null(ppStmt, index++);
5653  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5654  }
5655  else
5656  {
5657  rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
5658  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5659  }
5660  }
5661 
5662  if(uStrNumCmp(_version, "0.14.0") >= 0)
5663  {
5664  if(s->sensorData().gps().stamp() <= 0.0)
5665  {
5666  rc = sqlite3_bind_null(ppStmt, index++);
5667  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5668  }
5669  else
5670  {
5671  gps.resize(6,0.0);
5672  gps[0] = s->sensorData().gps().stamp();
5673  gps[1] = s->sensorData().gps().longitude();
5674  gps[2] = s->sensorData().gps().latitude();
5675  gps[3] = s->sensorData().gps().altitude();
5676  gps[4] = s->sensorData().gps().error();
5677  gps[5] = s->sensorData().gps().bearing();
5678  rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
5679  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5680  }
5681 
5682  if(uStrNumCmp(_version, "0.18.0") >= 0)
5683  {
5684  const EnvSensors & sensors = s->sensorData().envSensors();
5685  if(sensors.size() == 0)
5686  {
5687  rc = sqlite3_bind_null(ppStmt, index++);
5688  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5689  }
5690  else
5691  {
5692 
5693  envSensors.resize(sensors.size()*3,0.0);
5694  int j=0;
5695  for(std::map<EnvSensor::Type, EnvSensor>::const_iterator iter=sensors.begin(); iter!=sensors.end(); ++iter, j+=3)
5696  {
5697  envSensors[j] = (double)iter->second.type();
5698  envSensors[j+1] = iter->second.value();
5699  envSensors[j+2] = iter->second.stamp();
5700  }
5701  rc = sqlite3_bind_blob(ppStmt, index++, envSensors.data(), envSensors.size()*sizeof(double), SQLITE_STATIC);
5702  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5703  }
5704  }
5705  }
5706  }
5707  }
5708  else if(uStrNumCmp(_version, "0.8.8") >= 0)
5709  {
5710  if(s->sensorData().userDataCompressed().empty())
5711  {
5712  rc = sqlite3_bind_null(ppStmt, index++);
5713  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5714  }
5715  else
5716  {
5717  rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
5718  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5719  }
5720  }
5721 
5722  //step
5723  rc=sqlite3_step(ppStmt);
5724  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5725 
5726  rc = sqlite3_reset(ppStmt);
5727  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5728 }
5729 
5730 std::string DBDriverSqlite3::queryStepImage() const
5731 {
5732  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5733  return "INSERT INTO Image(id, data) VALUES(?,?);";
5734 }
5735 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
5736  int id,
5737  const cv::Mat & imageBytes) const
5738 {
5739  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5740  UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
5741  if(!ppStmt)
5742  {
5743  UFATAL("");
5744  }
5745 
5746  int rc = SQLITE_OK;
5747  int index = 1;
5748 
5749  rc = sqlite3_bind_int(ppStmt, index++, id);
5750  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5751 
5752  if(!imageBytes.empty())
5753  {
5754  rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
5755  }
5756  else
5757  {
5758  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5759  }
5760  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5761 
5762  //step
5763  rc=sqlite3_step(ppStmt);
5764  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5765 
5766  rc = sqlite3_reset(ppStmt);
5767  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5768 }
5769 
5770 std::string DBDriverSqlite3::queryStepDepth() const
5771 {
5772  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5773  if(uStrNumCmp(_version, "0.8.11") >= 0)
5774  {
5775  return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
5776  }
5777  else if(uStrNumCmp(_version, "0.7.0") >= 0)
5778  {
5779  return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
5780  }
5781  else
5782  {
5783  return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
5784  }
5785 }
5786 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
5787 {
5788  UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
5789  UDEBUG("Save depth %d (size=%d) depth2d = %d",
5790  sensorData.id(),
5791  (int)sensorData.depthOrRightCompressed().cols,
5792  sensorData.laserScanCompressed().size());
5793  if(!ppStmt)
5794  {
5795  UFATAL("");
5796  }
5797 
5798  int rc = SQLITE_OK;
5799  int index = 1;
5800 
5801  rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
5802  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5803 
5804  if(!sensorData.depthOrRightCompressed().empty())
5805  {
5806  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
5807  }
5808  else
5809  {
5810  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5811  }
5812  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5813 
5814  float fx=0, fyOrBaseline=0, cx=0, cy=0;
5815  Transform localTransform = Transform::getIdentity();
5816  if(sensorData.cameraModels().size())
5817  {
5818  UASSERT_MSG(sensorData.cameraModels().size() == 1,
5819  uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
5820 
5821  fx = sensorData.cameraModels()[0].fx();
5822  fyOrBaseline = sensorData.cameraModels()[0].fy();
5823  cx = sensorData.cameraModels()[0].cx();
5824  cy = sensorData.cameraModels()[0].cy();
5825  localTransform = sensorData.cameraModels()[0].localTransform();
5826  }
5827  else if(sensorData.stereoCameraModels().size())
5828  {
5829  UASSERT_MSG(sensorData.stereoCameraModels().size() == 1,
5830  uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
5831  fx = sensorData.stereoCameraModels()[0].left().fx();
5832  fyOrBaseline = sensorData.stereoCameraModels()[0].baseline();
5833  cx = sensorData.stereoCameraModels()[0].left().cx();
5834  cy = sensorData.stereoCameraModels()[0].left().cy();
5835  localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
5836  }
5837 
5838  if(uStrNumCmp(_version, "0.7.0") >= 0)
5839  {
5840  rc = sqlite3_bind_double(ppStmt, index++, fx);
5841  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5842  rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
5843  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5844  rc = sqlite3_bind_double(ppStmt, index++, cx);
5845  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5846  rc = sqlite3_bind_double(ppStmt, index++, cy);
5847  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5848  }
5849  else
5850  {
5851  rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
5852  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5853  }
5854 
5855  rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
5856  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5857 
5858  if(!sensorData.laserScanCompressed().isEmpty())
5859  {
5860  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, (int)sensorData.laserScanCompressed().size(), SQLITE_STATIC);
5861  }
5862  else
5863  {
5864  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
5865  }
5866  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5867 
5868  if(uStrNumCmp(_version, "0.8.11") >= 0)
5869  {
5870  rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
5871  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5872  }
5873 
5874  //step
5875  rc=sqlite3_step(ppStmt);
5876  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5877 
5878  rc = sqlite3_reset(ppStmt);
5879  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5880 }
5881 
5882 std::string DBDriverSqlite3::queryStepCalibrationUpdate() const
5883 {
5884  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
5885  return "UPDATE Data SET calibration=? WHERE id=?;";
5886 }
5887 void DBDriverSqlite3::stepCalibrationUpdate(
5888  sqlite3_stmt * ppStmt,
5889  int nodeId,
5890  const std::vector<CameraModel> & models,
5891  const std::vector<StereoCameraModel> & stereoModels) const
5892 {
5893  if(!ppStmt)
5894  {
5895  UFATAL("");
5896  }
5897 
5898  int rc = SQLITE_OK;
5899  int index = 1;
5900 
5901  // calibration
5902  std::vector<unsigned char> calibrationData;
5903  std::vector<float> calibration;
5904  // multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
5905  // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
5906  if(models.size() && models[0].isValidForProjection())
5907  {
5908  if(uStrNumCmp(_version, "0.18.0") >= 0)
5909  {
5910  for(unsigned int i=0; i<models.size(); ++i)
5911  {
5912  UASSERT(models[i].isValidForProjection());
5913  std::vector<unsigned char> data = models[i].serialize();
5914  UASSERT(!data.empty());
5915  unsigned int oldSize = calibrationData.size();
5916  calibrationData.resize(calibrationData.size() + data.size());
5917  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
5918  }
5919  }
5920  else if(uStrNumCmp(_version, "0.11.2") >= 0)
5921  {
5922  calibration.resize(models.size() * (6+Transform().size()));
5923  for(unsigned int i=0; i<models.size(); ++i)
5924  {
5925  UASSERT(models[i].isValidForProjection());
5926  const Transform & localTransform = models[i].localTransform();
5927  calibration[i*(6+localTransform.size())] = models[i].fx();
5928  calibration[i*(6+localTransform.size())+1] = models[i].fy();
5929  calibration[i*(6+localTransform.size())+2] = models[i].cx();
5930  calibration[i*(6+localTransform.size())+3] = models[i].cy();
5931  calibration[i*(6+localTransform.size())+4] = models[i].imageWidth();
5932  calibration[i*(6+localTransform.size())+5] = models[i].imageHeight();
5933  memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
5934  }
5935  }
5936  else
5937  {
5938  calibration.resize(models.size() * (4+Transform().size()));
5939  for(unsigned int i=0; i<models.size(); ++i)
5940  {
5941  UASSERT(models[i].isValidForProjection());
5942  const Transform & localTransform = models[i].localTransform();
5943  calibration[i*(4+localTransform.size())] = models[i].fx();
5944  calibration[i*(4+localTransform.size())+1] = models[i].fy();
5945  calibration[i*(4+localTransform.size())+2] = models[i].cx();
5946  calibration[i*(4+localTransform.size())+3] = models[i].cy();
5947  memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
5948  }
5949  }
5950  }
5951  else if(stereoModels.size() && stereoModels[0].isValidForProjection())
5952  {
5953  if(uStrNumCmp(_version, "0.18.0") >= 0)
5954  {
5955  for(unsigned int i=0; i<stereoModels.size(); ++i)
5956  {
5957  UASSERT(stereoModels[i].isValidForProjection());
5958  std::vector<unsigned char> data = stereoModels[i].serialize();
5959  UASSERT(!data.empty());
5960  unsigned int oldSize = calibrationData.size();
5961  calibrationData.resize(calibrationData.size() + data.size());
5962  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
5963  }
5964  }
5965  else
5966  {
5967  UASSERT_MSG(stereoModels.size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
5968  const Transform & localTransform = stereoModels[0].left().localTransform();
5969  calibration.resize(7+localTransform.size());
5970  calibration[0] = stereoModels[0].left().fx();
5971  calibration[1] = stereoModels[0].left().fy();
5972  calibration[2] = stereoModels[0].left().cx();
5973  calibration[3] = stereoModels[0].left().cy();
5974  calibration[4] = stereoModels[0].baseline();
5975  calibration[5] = stereoModels[0].left().imageWidth();
5976  calibration[6] = stereoModels[0].left().imageHeight();
5977  memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
5978  }
5979  }
5980 
5981  if(calibrationData.size())
5982  {
5983  rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
5984  }
5985  else if(calibration.size())
5986  {
5987  rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
5988  }
5989  else
5990  {
5991  rc = sqlite3_bind_null(ppStmt, index++);
5992  }
5993  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5994 
5995  //id
5996  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
5997  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
5998 
5999  //step
6000  rc=sqlite3_step(ppStmt);
6001  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6002 
6003  rc = sqlite3_reset(ppStmt);
6004  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6005 }
6006 
6007 std::string DBDriverSqlite3::queryStepDepthUpdate() const
6008 {
6009  if(uStrNumCmp(_version, "0.10.0") < 0)
6010  {
6011  return "UPDATE Depth SET data=? WHERE id=?;";
6012  }
6013  else
6014  {
6015  return "UPDATE Data SET depth=? WHERE id=?;";
6016  }
6017 }
6018 void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const
6019 {
6020  if(!ppStmt)
6021  {
6022  UFATAL("");
6023  }
6024 
6025  int rc = SQLITE_OK;
6026  int index = 1;
6027 
6028  cv::Mat imageCompressed;
6029  if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
6030  {
6031  // compress
6032  imageCompressed = compressImage2(image, format);
6033  }
6034  else
6035  {
6036  imageCompressed = image;
6037  }
6038  if(!imageCompressed.empty())
6039  {
6040  rc = sqlite3_bind_blob(ppStmt, index++, imageCompressed.data, (int)imageCompressed.cols, SQLITE_STATIC);
6041  }
6042  else
6043  {
6044  rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
6045  }
6046  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6047 
6048  //id
6049  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6050  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6051 
6052  //step
6053  rc=sqlite3_step(ppStmt);
6054  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6055 
6056  rc = sqlite3_reset(ppStmt);
6057  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6058 }
6059 
6060 std::string DBDriverSqlite3::queryStepScanUpdate() const
6061 {
6062  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
6063  if(uStrNumCmp(_version, "0.11.10") >= 0)
6064  {
6065  return "UPDATE Data SET scan_info=?, scan=? WHERE id=?;";
6066  }
6067  else if(uStrNumCmp(_version, "0.10.7") >= 0)
6068  {
6069  return "UPDATE Data SET scan_max_pts=?, scan_max_range=?, scan=? WHERE id=?;";
6070  }
6071  else
6072  {
6073  return "UPDATE Data SET scan_max_pts=? scan=? WHERE id=?;";
6074  }
6075 }
6076 void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & scan) const
6077 {
6078  if(!ppStmt)
6079  {
6080  UFATAL("");
6081  }
6082 
6083  int rc = SQLITE_OK;
6084  int index = 1;
6085 
6086  std::vector<float> scanInfo;
6087  if(uStrNumCmp(_version, "0.11.10") >= 0)
6088  {
6089  if(scan.maxPoints() > 0 ||
6090  scan.rangeMax() > 0 ||
6091  (uStrNumCmp(_version, "0.16.1")>=0 && scan.format() != LaserScan::kUnknown) ||
6092  (!scan.localTransform().isNull() && !scan.localTransform().isIdentity()))
6093  {
6094  if(uStrNumCmp(_version, "0.16.1") >=0)
6095  {
6096  if(uStrNumCmp(_version, "0.18.0") >=0)
6097  {
6098  scanInfo.resize(7 + Transform().size());
6099  scanInfo[0] = scan.format();
6100  scanInfo[1] = scan.rangeMin();
6101  scanInfo[2] = scan.rangeMax();
6102  scanInfo[3] = scan.angleMin();
6103  scanInfo[4] = scan.angleMax();
6104  scanInfo[5] = scan.angleIncrement();
6105  scanInfo[6] = scan.maxPoints(); // only for backward compatibility
6106  const Transform & localTransform = scan.localTransform();
6107  memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
6108  }
6109  else
6110  {
6111  scanInfo.resize(3 + Transform().size());
6112  scanInfo[0] = scan.maxPoints();
6113  scanInfo[1] = scan.rangeMax();
6114  scanInfo[2] = scan.format();
6115  const Transform & localTransform = scan.localTransform();
6116  memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
6117  }
6118  }
6119  else
6120  {
6121  scanInfo.resize(2 + Transform().size());
6122  scanInfo[0] = scan.maxPoints();
6123  scanInfo[1] = scan.rangeMax();
6124  const Transform & localTransform = scan.localTransform();
6125  memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
6126  }
6127  }
6128 
6129  if(scanInfo.size())
6130  {
6131  rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
6132  }
6133  else
6134  {
6135  rc = sqlite3_bind_null(ppStmt, index++);
6136  }
6137  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6138 
6139  }
6140  else
6141  {
6142  // scan_max_pts
6143  rc = sqlite3_bind_int(ppStmt, index++, scan.maxPoints());
6144  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6145 
6146  // scan_max_range
6147  if(uStrNumCmp(_version, "0.10.7") >= 0)
6148  {
6149  rc = sqlite3_bind_double(ppStmt, index++, scan.rangeMax());
6150  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6151  }
6152  }
6153 
6154  // scan
6155  cv::Mat scanCompressed;
6156  if(scan.isCompressed())
6157  {
6158  scanCompressed = scan.data();
6159  }
6160  else
6161  {
6162  scanCompressed = compressData2(scan.data());
6163  }
6164  if(!scanCompressed.empty())
6165  {
6166  rc = sqlite3_bind_blob(ppStmt, index++, scanCompressed.data, scanCompressed.total(), SQLITE_STATIC);
6167  }
6168  else
6169  {
6170  rc = sqlite3_bind_null(ppStmt, index++);
6171  }
6172  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6173 
6174  //id
6175  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6176  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6177 
6178  //step
6179  rc=sqlite3_step(ppStmt);
6180  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6181 
6182  rc = sqlite3_reset(ppStmt);
6183  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6184 }
6185 
6186 std::string DBDriverSqlite3::queryStepSensorData() const
6187 {
6188  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
6189  if(uStrNumCmp(_version, "0.16.0") >= 0)
6190  {
6191  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(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
6192  }
6193  else if(uStrNumCmp(_version, "0.11.10") >= 0)
6194  {
6195  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
6196  }
6197  else if(uStrNumCmp(_version, "0.10.7") >= 0)
6198  {
6199  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
6200  }
6201  else if(uStrNumCmp(_version, "0.10.1") >= 0)
6202  {
6203  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
6204  }
6205  else
6206  {
6207  return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
6208  }
6209 }
6210 void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
6211  const SensorData & sensorData) const
6212 {
6213  UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
6214  UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
6215  sensorData.id(),
6216  (int)sensorData.imageCompressed().cols,
6217  (int)sensorData.depthOrRightCompressed().cols,
6218  sensorData.laserScanCompressed().size());
6219  if(!ppStmt)
6220  {
6221  UFATAL("");
6222  }
6223 
6224  int rc = SQLITE_OK;
6225  int index = 1;
6226 
6227  // id
6228  rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
6229  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6230 
6231  // image
6232  if(!sensorData.imageCompressed().empty())
6233  {
6234  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
6235  }
6236  else
6237  {
6238  rc = sqlite3_bind_null(ppStmt, index++);
6239  }
6240  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6241 
6242  // depth or right image
6243  if(!sensorData.depthOrRightCompressed().empty())
6244  {
6245  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
6246  }
6247  else
6248  {
6249  rc = sqlite3_bind_null(ppStmt, index++);
6250  }
6251  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6252 
6253  // calibration
6254  std::vector<unsigned char> calibrationData;
6255  std::vector<float> calibration;
6256  // multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
6257  // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
6258  if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
6259  {
6260  if(uStrNumCmp(_version, "0.18.0") >= 0)
6261  {
6262  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6263  {
6264  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6265  std::vector<unsigned char> data = sensorData.cameraModels()[i].serialize();
6266  UASSERT(!data.empty());
6267  unsigned int oldSize = calibrationData.size();
6268  calibrationData.resize(calibrationData.size() + data.size());
6269  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
6270  }
6271  }
6272  else if(uStrNumCmp(_version, "0.11.2") >= 0)
6273  {
6274  calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
6275  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6276  {
6277  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6278  const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
6279  calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
6280  calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
6281  calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
6282  calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
6283  calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
6284  calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
6285  memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
6286  }
6287  }
6288  else
6289  {
6290  calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
6291  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
6292  {
6293  UASSERT(sensorData.cameraModels()[i].isValidForProjection());
6294  const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
6295  calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
6296  calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
6297  calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
6298  calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
6299  memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
6300  }
6301  }
6302  }
6303  else if(sensorData.stereoCameraModels().size() && sensorData.stereoCameraModels()[0].isValidForProjection())
6304  {
6305  if(uStrNumCmp(_version, "0.18.0") >= 0)
6306  {
6307  for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
6308  {
6309  UASSERT(sensorData.stereoCameraModels()[i].isValidForProjection());
6310  std::vector<unsigned char> data = sensorData.stereoCameraModels()[i].serialize();
6311  UASSERT(!data.empty());
6312  unsigned int oldSize = calibrationData.size();
6313  calibrationData.resize(calibrationData.size() + data.size());
6314  memcpy(calibrationData.data()+oldSize, data.data(), data.size());
6315  }
6316  }
6317  else
6318  {
6319  UASSERT_MSG(sensorData.stereoCameraModels().size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
6320  const Transform & localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
6321  calibration.resize(7+localTransform.size());
6322  calibration[0] = sensorData.stereoCameraModels()[0].left().fx();
6323  calibration[1] = sensorData.stereoCameraModels()[0].left().fy();
6324  calibration[2] = sensorData.stereoCameraModels()[0].left().cx();
6325  calibration[3] = sensorData.stereoCameraModels()[0].left().cy();
6326  calibration[4] = sensorData.stereoCameraModels()[0].baseline();
6327  calibration[5] = sensorData.stereoCameraModels()[0].left().imageWidth();
6328  calibration[6] = sensorData.stereoCameraModels()[0].left().imageHeight();
6329  memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
6330  }
6331  }
6332 
6333  if(calibrationData.size())
6334  {
6335  rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
6336  }
6337  else if(calibration.size())
6338  {
6339  rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
6340  }
6341  else
6342  {
6343  rc = sqlite3_bind_null(ppStmt, index++);
6344  }
6345  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6346 
6347  std::vector<float> scanInfo;
6348  if(uStrNumCmp(_version, "0.11.10") >= 0)
6349  {
6350  if(sensorData.laserScanCompressed().maxPoints() > 0 ||
6351  sensorData.laserScanCompressed().rangeMax() > 0 ||
6352  (uStrNumCmp(_version, "0.16.1")>=0 && sensorData.laserScanCompressed().format() != LaserScan::kUnknown) ||
6353  (!sensorData.laserScanCompressed().localTransform().isNull() && !sensorData.laserScanCompressed().localTransform().isIdentity()))
6354  {
6355  if(uStrNumCmp(_version, "0.16.1") >=0)
6356  {
6357  if(uStrNumCmp(_version, "0.18.0") >=0)
6358  {
6359  scanInfo.resize(7 + Transform().size());
6360  scanInfo[0] = sensorData.laserScanCompressed().format();
6361  scanInfo[1] = sensorData.laserScanCompressed().rangeMin();
6362  scanInfo[2] = sensorData.laserScanCompressed().rangeMax();
6363  scanInfo[3] = sensorData.laserScanCompressed().angleMin();
6364  scanInfo[4] = sensorData.laserScanCompressed().angleMax();
6365  scanInfo[5] = sensorData.laserScanCompressed().angleIncrement();
6366  scanInfo[6] = sensorData.laserScanCompressed().maxPoints(); // only for backward compatibility
6367  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6368  memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
6369  }
6370  else
6371  {
6372  scanInfo.resize(3 + Transform().size());
6373  scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
6374  scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
6375  scanInfo[2] = sensorData.laserScanCompressed().format();
6376  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6377  memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
6378  }
6379  }
6380  else
6381  {
6382  scanInfo.resize(2 + Transform().size());
6383  scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
6384  scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
6385  const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
6386  memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
6387  }
6388  }
6389 
6390  if(scanInfo.size())
6391  {
6392  rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
6393  }
6394  else
6395  {
6396  rc = sqlite3_bind_null(ppStmt, index++);
6397  }
6398  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6399 
6400  }
6401  else
6402  {
6403  // scan_max_pts
6404  rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
6405  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6406 
6407  // scan_max_range
6408  if(uStrNumCmp(_version, "0.10.7") >= 0)
6409  {
6410  rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanCompressed().rangeMax());
6411  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6412  }
6413  }
6414 
6415  // scan
6416  if(!sensorData.laserScanCompressed().isEmpty())
6417  {
6418  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, sensorData.laserScanCompressed().size(), SQLITE_STATIC);
6419  }
6420  else
6421  {
6422  rc = sqlite3_bind_null(ppStmt, index++);
6423  }
6424  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6425 
6426  if(uStrNumCmp(_version, "0.10.1") >= 0)
6427  {
6428  // user_data
6429  if(!sensorData.userDataCompressed().empty())
6430  {
6431  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
6432  }
6433  else
6434  {
6435  rc = sqlite3_bind_null(ppStmt, index++);
6436  }
6437  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6438  }
6439 
6440  if(uStrNumCmp(_version, "0.11.10") >= 0)
6441  {
6442  //ground_cells
6443  if(sensorData.gridGroundCellsCompressed().empty())
6444  {
6445  rc = sqlite3_bind_null(ppStmt, index++);
6446  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6447  }
6448  else
6449  {
6450  // compress
6451  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
6452  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6453  }
6454 
6455  //obstacle_cells
6456  if(sensorData.gridObstacleCellsCompressed().empty())
6457  {
6458  rc = sqlite3_bind_null(ppStmt, index++);
6459  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6460  }
6461  else
6462  {
6463  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
6464  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6465  }
6466 
6467  if(uStrNumCmp(_version, "0.16.0") >= 0)
6468  {
6469  //empty_cells
6470  if(sensorData.gridEmptyCellsCompressed().empty())
6471  {
6472  rc = sqlite3_bind_null(ppStmt, index++);
6473  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6474  }
6475  else
6476  {
6477  rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
6478  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6479  }
6480  }
6481 
6482  //cell_size
6483  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
6484  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6485 
6486  //view_point
6487  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
6488  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6489  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
6490  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6491  rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
6492  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6493  }
6494 
6495  //step
6496  rc=sqlite3_step(ppStmt);
6497  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6498 
6499  rc = sqlite3_reset(ppStmt);
6500  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6501 }
6502 
6503 std::string DBDriverSqlite3::queryStepLinkUpdate() const
6504 {
6505  if(uStrNumCmp(_version, "0.13.0") >= 0)
6506  {
6507  return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
6508  }
6509  else if(uStrNumCmp(_version, "0.10.10") >= 0)
6510  {
6511  return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
6512  }
6513  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6514  {
6515  return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
6516  }
6517  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6518  {
6519  return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
6520  }
6521  else
6522  {
6523  return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
6524  }
6525 }
6526 std::string DBDriverSqlite3::queryStepLink() const
6527 {
6528  // from_id, to_id are at the end to match the update query above
6529  if(uStrNumCmp(_version, "0.13.0") >= 0)
6530  {
6531  return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
6532  }
6533  else if(uStrNumCmp(_version, "0.10.10") >= 0)
6534  {
6535  return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
6536  }
6537  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6538  {
6539  return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
6540  }
6541  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6542  {
6543  return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
6544  }
6545  else
6546  {
6547  return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
6548  }
6549 }
6550 void DBDriverSqlite3::stepLink(
6551  sqlite3_stmt * ppStmt,
6552  const Link & link) const
6553 {
6554  if(!ppStmt)
6555  {
6556  UFATAL("");
6557  }
6558  UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
6559 
6560  // Don't save virtual links
6561  if(link.type()==Link::kVirtualClosure)
6562  {
6563  UDEBUG("Virtual link ignored....");
6564  return;
6565  }
6566 
6567  int rc = SQLITE_OK;
6568  int index = 1;
6569  rc = sqlite3_bind_int(ppStmt, index++, link.type());
6570  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6571 
6572  if(uStrNumCmp(_version, "0.13.0") >= 0)
6573  {
6574  // information_matrix
6575  rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
6576  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6577  }
6578  else if(uStrNumCmp(_version, "0.8.4") >= 0)
6579  {
6580  rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
6581  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6582  rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
6583  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6584  }
6585  else if(uStrNumCmp(_version, "0.7.4") >= 0)
6586  {
6587  rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
6588  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6589  }
6590 
6591  rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
6592  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6593 
6594  if(uStrNumCmp(_version, "0.10.10") >= 0)
6595  {
6596  // user_data
6597  if(!link.userDataCompressed().empty())
6598  {
6599  rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
6600  }
6601  else
6602  {
6603  rc = sqlite3_bind_null(ppStmt, index++);
6604  }
6605  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6606  }
6607 
6608  rc = sqlite3_bind_int(ppStmt, index++, link.from());
6609  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6610  rc = sqlite3_bind_int(ppStmt, index++, link.to());
6611  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6612 
6613  rc=sqlite3_step(ppStmt);
6614  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6615 
6616  rc=sqlite3_reset(ppStmt);
6617  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6618 }
6619 
6620 std::string DBDriverSqlite3::queryStepWordsChanged() const
6621 {
6622  if(uStrNumCmp(_version, "0.13.0") >= 0)
6623  {
6624  return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
6625  }
6626  else
6627  {
6628  return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
6629  }
6630 }
6631 void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
6632 {
6633  if(!ppStmt)
6634  {
6635  UFATAL("");
6636  }
6637  int rc = SQLITE_OK;
6638  int index = 1;
6639  rc = sqlite3_bind_int(ppStmt, index++, newWordId);
6640  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6641  rc = sqlite3_bind_int(ppStmt, index++, oldWordId);
6642  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6643  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6644  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6645 
6646  rc=sqlite3_step(ppStmt);
6647  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6648 
6649  rc=sqlite3_reset(ppStmt);
6650  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6651 }
6652 
6653 std::string DBDriverSqlite3::queryStepKeypoint() const
6654 {
6655  if(uStrNumCmp(_version, "0.13.0") >= 0)
6656  {
6657  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
6658  }
6659  else if(uStrNumCmp(_version, "0.12.0") >= 0)
6660  {
6661  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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
6662  }
6663  else if(uStrNumCmp(_version, "0.11.2") >= 0)
6664  {
6665  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(?,?,?,?,?,?,?,?,?,?,?,?);";
6666  }
6667  return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
6668 }
6669 void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
6670  int nodeId,
6671  int wordId,
6672  const cv::KeyPoint & kp,
6673  const cv::Point3f & pt,
6674  const cv::Mat & descriptor) const
6675 {
6676  if(!ppStmt)
6677  {
6678  UFATAL("");
6679  }
6680  int rc = SQLITE_OK;
6681  int index = 1;
6682  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6683  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6684  rc = sqlite3_bind_int(ppStmt, index++, wordId);
6685  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6686  rc = sqlite3_bind_double(ppStmt, index++, kp.pt.x);
6687  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6688  rc = sqlite3_bind_double(ppStmt, index++, kp.pt.y);
6689  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6690  rc = sqlite3_bind_int(ppStmt, index++, kp.size);
6691  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6692  rc = sqlite3_bind_double(ppStmt, index++, kp.angle);
6693  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6694  rc = sqlite3_bind_double(ppStmt, index++, kp.response);
6695  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6696  if(uStrNumCmp(_version, "0.12.0") >= 0)
6697  {
6698  rc = sqlite3_bind_int(ppStmt, index++, kp.octave);
6699  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6700  }
6701 
6702  if(uIsFinite(pt.x))
6703  {
6704  rc = sqlite3_bind_double(ppStmt, index++, pt.x);
6705  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6706  }
6707  else
6708  {
6709  rc = sqlite3_bind_null(ppStmt, index++);
6710  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6711  }
6712 
6713  if(uIsFinite(pt.y))
6714  {
6715  rc = sqlite3_bind_double(ppStmt, index++, pt.y);
6716  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6717  }
6718  else
6719  {
6720  rc = sqlite3_bind_null(ppStmt, index++);
6721  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6722  }
6723 
6724  if(uIsFinite(pt.z))
6725  {
6726  rc = sqlite3_bind_double(ppStmt, index++, pt.z);
6727  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6728  }
6729  else
6730  {
6731  rc = sqlite3_bind_null(ppStmt, index++);
6732  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6733  }
6734 
6735  //descriptor
6736  if(uStrNumCmp(_version, "0.11.2") >= 0)
6737  {
6738  rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
6739  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6740  UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
6741  if(descriptor.empty())
6742  {
6743  rc = sqlite3_bind_null(ppStmt, index++);
6744  }
6745  else
6746  {
6747  if(descriptor.type() == CV_32F)
6748  {
6749  // CV_32F
6750  rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
6751  }
6752  else
6753  {
6754  // CV_8U
6755  rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
6756  }
6757  }
6758  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6759  }
6760 
6761  rc=sqlite3_step(ppStmt);
6762  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6763 
6764  rc = sqlite3_reset(ppStmt);
6765  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6766 }
6767 
6768 std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
6769 {
6770  UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
6771  return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
6772 }
6773 void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
6774  int nodeId,
6775  const GlobalDescriptor & descriptor) const
6776 {
6777  if(!ppStmt)
6778  {
6779  UFATAL("");
6780  }
6781  int rc = SQLITE_OK;
6782  int index = 1;
6783 
6784  //node_if
6785  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6786  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6787 
6788  //type
6789  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6790  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6791 
6792  //info
6793  std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
6794  if(infoBytes.empty())
6795  {
6796  rc = sqlite3_bind_null(ppStmt, index++);
6797  }
6798  else
6799  {
6800  rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
6801  }
6802  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6803 
6804  //data
6805  std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
6806  if(dataBytes.empty())
6807  {
6808  rc = sqlite3_bind_null(ppStmt, index++);
6809  }
6810  else
6811  {
6812  rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
6813  }
6814  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6815 
6816  rc=sqlite3_step(ppStmt);
6817  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6818 
6819  rc = sqlite3_reset(ppStmt);
6820  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6821 }
6822 
6823 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
6824 {
6825  UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
6826  if(uStrNumCmp(_version, "0.16.0") >= 0)
6827  {
6828  return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
6829  }
6830  return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
6831 }
6832 void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
6833  int nodeId,
6834  const cv::Mat & ground,
6835  const cv::Mat & obstacles,
6836  const cv::Mat & empty,
6837  float cellSize,
6838  const cv::Point3f & viewpoint) const
6839 {
6840  UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
6841  UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
6842  UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
6843  UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
6844  UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
6845  nodeId,
6846  ground.cols,
6847  obstacles.cols,
6848  empty.cols,
6849  cellSize,
6850  viewpoint.x,
6851  viewpoint.y,
6852  viewpoint.z);
6853  if(!ppStmt)
6854  {
6855  UFATAL("");
6856  }
6857 
6858  int rc = SQLITE_OK;
6859  int index = 1;
6860 
6861  //ground_cells
6862  if(ground.empty())
6863  {
6864  rc = sqlite3_bind_null(ppStmt, index++);
6865  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6866  }
6867  else
6868  {
6869  // compress
6870  rc = sqlite3_bind_blob(ppStmt, index++, ground.data, ground.cols, SQLITE_STATIC);
6871  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6872  }
6873 
6874  //obstacle_cells
6875  if(obstacles.empty())
6876  {
6877  rc = sqlite3_bind_null(ppStmt, index++);
6878  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6879  }
6880  else
6881  {
6882  rc = sqlite3_bind_blob(ppStmt, index++, obstacles.data, obstacles.cols, SQLITE_STATIC);
6883  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6884  }
6885 
6886  if(uStrNumCmp(_version, "0.16.0") >= 0)
6887  {
6888  //empty_cells
6889  if(empty.empty())
6890  {
6891  rc = sqlite3_bind_null(ppStmt, index++);
6892  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6893  }
6894  else
6895  {
6896  rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
6897  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6898  }
6899  }
6900 
6901  //cell_size
6902  rc = sqlite3_bind_double(ppStmt, index++, cellSize);
6903  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6904 
6905  //view_point
6906  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.x);
6907  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6908  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.y);
6909  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6910  rc = sqlite3_bind_double(ppStmt, index++, viewpoint.z);
6911  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6912 
6913  // id
6914  rc = sqlite3_bind_int(ppStmt, index++, nodeId);
6915  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6916 
6917 
6918  //step
6919  rc=sqlite3_step(ppStmt);
6920  UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6921 
6922  rc = sqlite3_reset(ppStmt);
6923  UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
6924 }
6925 
6926 } // 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:6076
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:201
save
save
rtabmap::uncompressImage
cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:144
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:5236
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:5569
rtabmap::DBDriverSqlite3::loadWordsQuery
virtual void loadWordsQuery(const std::set< int > &wordIds, std::list< VisualWord * > &vws) const
Definition: DBDriverSqlite3.cpp:3746
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:206
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::updateDepthImageQuery
virtual void updateDepthImageQuery(int nodeId, const cv::Mat &image, const std::string &format) const
Definition: DBDriverSqlite3.cpp:4724
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:4691
fx
const double fx
rtabmap::DBDriverSqlite3::queryStepKeypoint
std::string queryStepKeypoint() const
Definition: DBDriverSqlite3.cpp:6653
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:3607
rtabmap::DBDriverSqlite3::load2DMapQuery
virtual cv::Mat load2DMapQuery(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriverSqlite3.cpp:5178
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:5735
rtabmap::DBDriverSqlite3::saveQuery
virtual void saveQuery(const std::list< Signature * > &signatures)
Definition: DBDriverSqlite3.cpp:4354
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
rtabmap::DBDriverSqlite3::getAllOdomPosesQuery
virtual void getAllOdomPosesQuery(std::map< int, Transform > &poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
Definition: DBDriverSqlite3.cpp:2490
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:4597
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:5887
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::DBDriverSqlite3::stepDepthUpdate
void stepDepthUpdate(sqlite3_stmt *ppStmt, int nodeId, const cv::Mat &image, const std::string &format) const
Definition: DBDriverSqlite3.cpp:6018
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:208
rtabmap::DBDriverSqlite3::queryStepCalibrationUpdate
std::string queryStepCalibrationUpdate() const
Definition: DBDriverSqlite3.cpp:5882
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:4652
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:6060
rtabmap::DBDriverSqlite3::getInvertedIndexNiQuery
virtual void getInvertedIndexNiQuery(int signatureId, int &ni) const
Definition: DBDriverSqlite3.cpp:2734
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:5786
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:4919
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:4869
rtabmap::DBDriverSqlite3::_dbInMemory
bool _dbInMemory
Definition: DBDriverSqlite3.h:202
Signature.h
rtabmap::DBDriverSqlite3::loadOptimizedPosesQuery
virtual std::map< int, Transform > loadOptimizedPosesQuery(Transform *lastlocalizationPose=0) const
Definition: DBDriverSqlite3.cpp:5043
rtabmap::DBDriverSqlite3::updateQuery
virtual void updateQuery(const std::list< Signature * > &signatures, bool updateTimestamp) const
Definition: DBDriverSqlite3.cpp:4164
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:1043
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:3436
rtabmap::DBDriverSqlite3::_journalMode
int _journalMode
Definition: DBDriverSqlite3.h:204
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::DBDriverSqlite3::queryStepLinkUpdate
std::string queryStepLinkUpdate() const
Definition: DBDriverSqlite3.cpp:6503
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:2559
sqlite3_column_bytes
#define sqlite3_column_bytes
Definition: sqlite3ext.h:289
rtabmap::DBDriverSqlite3::queryStepLink
std::string queryStepLink() const
Definition: DBDriverSqlite3.cpp:6526
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:5432
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:203
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:6186
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:6823
rtabmap::DBDriverSqlite3::stepNode
void stepNode(sqlite3_stmt *ppStmt, const Signature *s) const
Definition: DBDriverSqlite3.cpp:5601
rtabmap::GlobalDescriptor
Definition: GlobalDescriptor.h:35
rtabmap::DBDriverSqlite3::getNodeIdByLabelQuery
virtual void getNodeIdByLabelQuery(const std::string &label, int &id) const
Definition: DBDriverSqlite3.cpp:2863
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:4757
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:198
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:205
rtabmap::DBDriverSqlite3::queryStepWordsChanged
std::string queryStepWordsChanged() const
Definition: DBDriverSqlite3.cpp:6620
rtabmap::DBDriverSqlite3::updateLinkQuery
virtual void updateLinkQuery(const Link &link) const
Definition: DBDriverSqlite3.cpp:4625
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:239
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:2783
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:5770
rtabmap::compressString
cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string &str)
Definition: Compression.cpp:315
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:6550
rtabmap::DBDriverSqlite3::saveOptimizedPosesQuery
virtual void saveOptimizedPosesQuery(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriverSqlite3.cpp:4970
rtabmap::DBDriverSqlite3::getWeightQuery
virtual void getWeightQuery(int signatureId, int &weight) const
Definition: DBDriverSqlite3.cpp:2933
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:5730
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:3658
rtabmap::DBDriverSqlite3::addStatisticsQuery
virtual void addStatisticsQuery(const Statistics &statistics, bool saveWmState) const
Definition: DBDriverSqlite3.cpp:4788
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:2968
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:5126
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:6210
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:6631
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:6007
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:3841
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
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:134
rtabmap::DBDriverSqlite3::stepGlobalDescriptor
void stepGlobalDescriptor(sqlite3_stmt *ppStmt, int nodeId, const GlobalDescriptor &descriptor) const
Definition: DBDriverSqlite3.cpp:6773
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
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:2893
rtabmap::uncompressString
std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat &bytes)
Definition: Compression.cpp:321
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:2695
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:6669
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:197
rtabmap::Statistics::stamp
double stamp() const
Definition: Statistics.h:270
rtabmap::DBDriverSqlite3::queryStepGlobalDescriptor
std::string queryStepGlobalDescriptor() const
Definition: DBDriverSqlite3.cpp:6768
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:6832
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:269

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:53