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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31