i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
de4b17332df3fba67ea8b9c3cb040aadaba67d30
rtabmap/corelib/src/DBDriverSqlite3.cpp

6470 lines
219 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/DBDriverSqlite3.h"
#include <sqlite3.h>
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/VisualWord.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include "DatabaseSchema_sql.h"
#include <set>
#include "rtabmap/utilite/UtiLite.h"
namespace rtabmap {
DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
DBDriver(parameters),
_ppDb(0),
_version("0.0.0"),
_memoryUsedEstimate(0),
_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
_journalMode(Parameters::defaultDbSqlite3JournalMode()),
_synchronous(Parameters::defaultDbSqlite3Synchronous()),
_tempStore(Parameters::defaultDbSqlite3TempStore())
{
ULOGGER_DEBUG("treadSafe=%d", sqlite3_threadsafe());
this->parseParameters(parameters);
}
DBDriverSqlite3::~DBDriverSqlite3()
{
this->closeConnection();
}
void DBDriverSqlite3::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kDbSqlite3CacheSize())) != parameters.end())
{
this->setCacheSize(std::atoi((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kDbSqlite3JournalMode())) != parameters.end())
{
this->setJournalMode(std::atoi((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kDbSqlite3Synchronous())) != parameters.end())
{
this->setSynchronous(std::atoi((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kDbSqlite3TempStore())) != parameters.end())
{
this->setTempStore(std::atoi((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kDbSqlite3InMemory())) != parameters.end())
{
this->setDbInMemory(uStr2Bool((*iter).second.c_str()));
}
DBDriver::parseParameters(parameters);
}
void DBDriverSqlite3::setCacheSize(unsigned int cacheSize)
{
if(this->isConnected())
{
_cacheSize = cacheSize;
std::string query = "PRAGMA cache_size = ";
query += uNumber2Str(_cacheSize) + ";";
this->executeNoResultQuery(query.c_str());
}
}
void DBDriverSqlite3::setJournalMode(int journalMode)
{
if(journalMode >= 0 && journalMode < 5)
{
_journalMode = journalMode;
if(this->isConnected())
{
switch(_journalMode)
{
case 4:
this->executeNoResultQuery("PRAGMA journal_mode = OFF;");
break;
case 3:
this->executeNoResultQuery("PRAGMA journal_mode = MEMORY;");
break;
case 2:
this->executeNoResultQuery("PRAGMA journal_mode = PERSIST;");
break;
case 1:
this->executeNoResultQuery("PRAGMA journal_mode = TRUNCATE;");
break;
case 0:
default:
this->executeNoResultQuery("PRAGMA journal_mode = DELETE;");
break;
}
}
}
else
{
ULOGGER_ERROR("Wrong journal mode (%d)", journalMode);
}
}
void DBDriverSqlite3::setSynchronous(int synchronous)
{
if(synchronous >= 0 && synchronous < 3)
{
_synchronous = synchronous;
if(this->isConnected())
{
switch(_synchronous)
{
case 0:
this->executeNoResultQuery("PRAGMA synchronous = OFF;");
break;
case 1:
this->executeNoResultQuery("PRAGMA synchronous = NORMAL;");
break;
case 2:
default:
this->executeNoResultQuery("PRAGMA synchronous = FULL;");
break;
}
}
}
else
{
ULOGGER_ERROR("Wrong synchronous value (%d)", synchronous);
}
}
void DBDriverSqlite3::setTempStore(int tempStore)
{
if(tempStore >= 0 && tempStore < 3)
{
_tempStore = tempStore;
if(this->isConnected())
{
switch(_tempStore)
{
case 2:
this->executeNoResultQuery("PRAGMA temp_store = MEMORY;");
break;
case 1:
this->executeNoResultQuery("PRAGMA temp_store = FILE;");
break;
case 0:
default:
this->executeNoResultQuery("PRAGMA temp_store = DEFAULT;");
break;
}
}
}
else
{
ULOGGER_ERROR("Wrong tempStore value (%d)", tempStore);
}
}
void DBDriverSqlite3::setDbInMemory(bool dbInMemory)
{
UDEBUG("dbInMemory=%d", dbInMemory?1:0);
if(dbInMemory != _dbInMemory)
{
if(this->isConnected())
{
// Hard reset...
join(true);
this->emptyTrashes();
this->closeConnection();
_dbInMemory = dbInMemory;
this->openConnection(this->getUrl());
}
else
{
_dbInMemory = dbInMemory;
}
}
}
/*
** This function is used to load the contents of a database file on disk
** into the "main" database of open database connection pInMemory, or
** to save the current contents of the database opened by pInMemory into
** a database file on disk. pInMemory is probably an in-memory database,
** but this function will also work fine if it is not.
**
** Parameter zFilename points to a nul-terminated string containing the
** name of the database file on disk to load from or save to. If parameter
** isSave is non-zero, then the contents of the file zFilename are
** overwritten with the contents of the database opened by pInMemory. If
** parameter isSave is zero, then the contents of the database opened by
** pInMemory are replaced by data loaded from the file zFilename.
**
** If the operation is successful, SQLITE_OK is returned. Otherwise, if
** an error occurs, an SQLite error code is returned.
*/
int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const
{
int rc; /* Function return code */
sqlite3 *pFile = 0; /* Database connection opened on zFilename */
sqlite3_backup *pBackup = 0; /* Backup object used to copy data */
sqlite3 *pTo = 0; /* Database to copy to (pFile or pInMemory) */
sqlite3 *pFrom = 0; /* Database to copy from (pFile or pInMemory) */
/* Open the database file identified by zFilename. Exit early if this fails
** for any reason. */
rc = sqlite3_open(fileName.c_str(), &pFile);
if( rc==SQLITE_OK ){
/* If this is a 'load' operation (isSave==0), then data is copied
** from the database file just opened to database pInMemory.
** Otherwise, if this is a 'save' operation (isSave==1), then data
** is copied from pInMemory to pFile. Set the variables pFrom and
** pTo accordingly. */
pFrom = (isSave ? pInMemory : pFile);
pTo = (isSave ? pFile : pInMemory);
/* Set up the backup procedure to copy from the "main" database of
** connection pFile to the main database of connection pInMemory.
** If something goes wrong, pBackup will be set to NULL and an error
** code and message left in connection pTo.
**
** If the backup object is successfully created, call backup_step()
** to copy data from pFile to pInMemory. Then call backup_finish()
** to release resources associated with the pBackup object. If an
** error occurred, then an error code and message will be left in
** connection pTo. If no error occurred, then the error code belonging
** to pTo is set to SQLITE_OK.
*/
pBackup = sqlite3_backup_init(pTo, "main", pFrom, "main");
if( pBackup ){
(void)sqlite3_backup_step(pBackup, -1);
(void)sqlite3_backup_finish(pBackup);
}
rc = sqlite3_errcode(pTo);
}
/* Close the database connection opened on database file zFilename
** and return the result of this function. */
(void)sqlite3_close(pFile);
return rc;
}
bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
{
version = "0.0.0";
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT version FROM Admin;";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
if(rc == SQLITE_OK)
{
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//else
//{
// old version detected
//}
return true;
}
return false;
}
bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
{
this->disconnectDatabaseQuery();
// Open a database connection
_ppDb = 0;
_memoryUsedEstimate = 0;
int rc = SQLITE_OK;
bool dbFileExist = false;
if(!url.empty())
{
dbFileExist = UFile::exists(url.c_str());
if(dbFileExist && overwritten)
{
UINFO("Deleting database %s...", url.c_str());
UASSERT(UFile::erase(url.c_str()) == 0);
dbFileExist = false;
}
else if(dbFileExist)
{
_memoryUsedEstimate = UFile::length(this->getUrl());
}
}
if(_dbInMemory || url.empty())
{
if(!url.empty())
{
ULOGGER_INFO("Using database \"%s\" in the memory.", url.c_str());
}
else
{
ULOGGER_INFO("Using empty database in the memory.");
}
rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
}
else
{
ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
}
if(rc != SQLITE_OK)
{
UFATAL("DB error : %s (path=\"%s\"). Make sure that your user has write "
"permission on the target directory (you may have to change the working directory). ", sqlite3_errmsg(_ppDb), url.c_str());
_ppDb = 0;
return false;
}
if(_dbInMemory && dbFileExist)
{
UTimer timer;
timer.start();
ULOGGER_DEBUG("Loading DB ...");
rc = loadOrSaveDb(_ppDb, url, 0); // Load memory from file
ULOGGER_INFO("Loading DB time = %fs, (%s)", timer.ticks(), url.c_str());
if(rc != SQLITE_OK)
{
UFATAL("DB error 2 : %s", sqlite3_errmsg(_ppDb));
sqlite3_close(_ppDb);
_ppDb = 0;
return false;
}
}
if(!dbFileExist)
{
if(!url.empty())
{
ULOGGER_INFO("Database \"%s\" doesn't exist, creating a new one...", url.c_str());
}
// Create the database
std::string schema = DATABASESCHEMA_SQL;
schema = uHex2Str(schema);
this->executeNoResultQuery(schema.c_str());
}
UASSERT(this->getDatabaseVersionQuery(_version)); // must be true!
UINFO("Database version = %s", _version.c_str());
// From 0.11.13, compare only with minor version (patch will be used for non-database structural changes)
if((uStrNumCmp(_version, "0.11.12") <= 0 && uStrNumCmp(_version, RTABMAP_VERSION) > 0) ||
(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))
{
UERROR("Opened database version (%s) is more recent than rtabmap "
"installed version (%s). Please update rtabmap to new version!",
_version.c_str(), RTABMAP_VERSION);
this->disconnectDatabaseQuery(false);
return false;
}
//Set database optimizations
this->setCacheSize(_cacheSize); // this will call the SQL
this->setJournalMode(_journalMode); // this will call the SQL
this->setSynchronous(_synchronous); // this will call the SQL
this->setTempStore(_tempStore); // this will call the SQL
return true;
}
void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
{
UDEBUG("");
if(_ppDb)
{
int rc = SQLITE_OK;
// make sure that all statements are finalized
sqlite3_stmt * pStmt;
while( (pStmt = sqlite3_next_stmt(_ppDb, 0))!=0 )
{
rc = sqlite3_finalize(pStmt);
if(rc != SQLITE_OK)
{
UERROR("");
}
}
if(save && (_dbInMemory || this->getUrl().empty()))
{
UTimer timer;
timer.start();
std::string outputFile = this->getUrl();
if(!outputUrl.empty())
{
outputFile = outputUrl;
}
if(outputFile.empty())
{
UWARN("Database was initialized with an empty url (in memory). To save it, "
"the output url should not be empty. The database is thus closed without being saved!");
}
else
{
UINFO("Saving database to %s ...", outputFile.c_str());
rc = loadOrSaveDb(_ppDb, outputFile, 1); // Save memory to file
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s), could not save \"%s\": %s. Make sure that your user has write "
"permission on the target directory (you may have to change the working directory). ", _version.c_str(), outputFile.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
}
}
// Then close (delete) the database connection
UINFO("Disconnecting database %s...", this->getUrl().c_str());
sqlite3_close(_ppDb);
_ppDb = 0;
if(save && !_dbInMemory && !outputUrl.empty() && !this->getUrl().empty() && outputUrl.compare(this->getUrl()) != 0)
{
UWARN("Output database path (%s) is different than the opened database "
"path (%s). Opened database path is overwritten then renamed to output path.",
outputUrl.c_str(), this->getUrl().c_str());
if(UFile::rename(this->getUrl(), outputUrl) != 0)
{
UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
}
}
}
}
bool DBDriverSqlite3::isConnectedQuery() const
{
return _ppDb != 0;
}
// In bytes
void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc;
rc = sqlite3_exec(_ppDb, sql.c_str(), 0, 0, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s, the query is %s", sqlite3_errmsg(_ppDb), sql.c_str()).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
long DBDriverSqlite3::getMemoryUsedQuery() const
{
if(_dbInMemory)
{
return sqlite3_memory_used();
}
else
{
return _memoryUsedEstimate;
}
}
long DBDriverSqlite3::getNodesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose) + ifnull(length(velocity),0) + ifnull(length(gps),0) + ifnull(length(env_sensors),0) + length(time_enter)) from Node;";
}
else if(uStrNumCmp(_version, "0.14.0") >= 0)
{
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;";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
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;";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
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;";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(time_enter)) from Node;";
}
else
{
query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose)+ length(time_enter)) from Node;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getLinksMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query = "SELECT sum(length(type) + length(information_matrix) + length(transform) + ifnull(length(user_data),0) + length(from_id) + length(to_id)) from Link;";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
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;";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query = "SELECT sum(length(type) + length(rot_variance) + length(trans_variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query = "SELECT sum(length(type) + length(variance) + length(transform) + length(from_id) + length(to_id)) from Link;";
}
else
{
query = "SELECT sum(length(type) + length(transform) + length(from_id) + length(to_id)) from Link;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getImagesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query = "SELECT sum(ifnull(length(image),0) + ifnull(length(time_enter),0)) from Data;";
}
else
{
query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Image;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getDepthImagesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query = "SELECT sum(ifnull(length(depth),0) + ifnull(length(time_enter),0)) from Data;";
}
else
{
query = "SELECT sum(length(data) + ifnull(length(time_enter),0)) from Depth;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getCalibrationsMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query = "SELECT sum(length(calibration)) from Data;";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query = "SELECT sum(length(fx) + length(fy) + length(cx) + length(cy) + length(local_transform)) from Depth;";
}
else
{
query = "SELECT sum(length(constant) + length(local_transform)) from Depth;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getGridsMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
query = "SELECT sum(ifnull(length(ground_cells),0) + ifnull(length(obstacle_cells),0) + ifnull(length(empty_cells),0) + length(cell_size) + length(view_point_x) + length(view_point_y) + length(view_point_z)) from Data;";
}
else if(uStrNumCmp(_version, "0.11.10") >= 0)
{
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;";
}
else
{
return size;
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getLaserScansMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
query = "SELECT sum(ifnull(length(scan_info),0) + ifnull(length(scan),0)) from Data;";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
query = "SELECT sum(length(scan_max_pts) + length(scan_max_range) + ifnull(length(scan),0)) from Data;";
}
else if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query = "SELECT sum(length(scan_max_pts) + ifnull(length(scan),0)) from Data;";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query = "SELECT sum(length(data2d) + length(data2d_max_pts)) from Depth;";
}
else
{
query = "SELECT sum(length(data2d)) from Depth;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getUserDataMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query = "SELECT sum(length(user_data)) from Data;";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query = "SELECT sum(length(user_data)) from Node;";
}
else
{
return size; // no user_data
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getWordsMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query = "SELECT sum(length(id) + length(descriptor_size) + length(descriptor) + length(time_enter)) from Word;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
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)) "
"FROM Feature";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
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)) "
"FROM Map_Node_Word";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
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)) "
"FROM Map_Node_Word";
}
else
{
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)) "
"FROM Map_Node_Word";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getStatisticsMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.16.2") >= 0)
{
query = "SELECT sum(length(id) + length(stamp) + ifnull(length(data),0) + ifnull(length(wm_state),0)) FROM Statistics;";
}
else if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT sum(length(id) + length(stamp) + length(data)) FROM Statistics;";
}
else
{
return size;
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getLastNodesSizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
}
else
{
query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getLastDictionarySizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
}
else
{
query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getTotalNodesSizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Node;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getTotalDictionarySizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Word;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
ParametersMap DBDriverSqlite3::getLastParametersQuery() const
{
UDEBUG("");
ParametersMap parameters;
if(_ppDb)
{
if(uStrNumCmp(_version, "0.11.8") >= 0)
{
std::string query;
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT parameters "
"FROM Info "
"WHERE time_enter >= (SELECT MAX(time_enter) FROM Info);";
}
else
{
query = "SELECT parameters "
"FROM Statistics "
"WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
std::string text((const char *)sqlite3_column_text(ppStmt, 0));
if(text.size())
{
parameters = Parameters::deserialize(text);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
return parameters;
}
std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const
{
UDEBUG("nodeId=%d", nodeId);
std::map<std::string, float> data;
if(_ppDb)
{
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
std::stringstream query;
if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
{
query << "SELECT stamp, data, wm_state "
<< "FROM Statistics "
<< "WHERE id=" << nodeId << ";";
}
else
{
query << "SELECT stamp, data "
<< "FROM Statistics "
<< "WHERE id=" << nodeId << ";";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
int index = 0;
stamp = sqlite3_column_double(ppStmt, index++);
std::string text;
if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
{
const void * dataPtr = sqlite3_column_blob(ppStmt, index);
int dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
}
}
else
{
text = (const char *)sqlite3_column_text(ppStmt, index++);
}
if(text.size())
{
data = Statistics::deserializeData(text);
}
if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
{
const void * dataPtr = sqlite3_column_blob(ppStmt, index);
int dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
wmState->resize(wmStateMat.cols);
memcpy(wmState->data(), wmStateMat.data, wmState->size()*sizeof(int));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
return data;
}
std::map<int, std::pair<std::map<std::string, float>, double> > DBDriverSqlite3::getAllStatisticsQuery() const
{
UDEBUG("");
std::map<int, std::pair<std::map<std::string, float>, double> > data;
if(_ppDb)
{
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
std::stringstream query;
query << "SELECT id, stamp, data "
<< "FROM Statistics;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
int id = sqlite3_column_int(ppStmt, index++);
double stamp = sqlite3_column_double(ppStmt, index++);
std::string text;
if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
{
const void * dataPtr = 0;
int dataSize = 0;
dataPtr = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
}
}
else
{
text = (const char *)sqlite3_column_text(ppStmt, index++);
}
if(text.size())
{
data.insert(std::make_pair(id, std::make_pair(Statistics::deserializeData(text), stamp)));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
UDEBUG("");
return data;
}
std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery() const
{
UDEBUG("");
std::map<int, std::vector<int> > data;
if(_ppDb)
{
if(uStrNumCmp(_version, "0.16.2") >= 0)
{
std::stringstream query;
query << "SELECT id, wm_state "
<< "FROM Statistics;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
int id = sqlite3_column_int(ppStmt, index++);
std::vector<int> wmState;
const void * dataPtr = sqlite3_column_blob(ppStmt, index);
int dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
wmState.resize(wmStateMat.cols);
memcpy(wmState.data(), wmStateMat.data, wmState.size()*sizeof(int));
}
if(!wmState.empty())
{
data.insert(std::make_pair(id, wmState));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
UDEBUG("");
return data;
}
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("load data for %d signatures", (int)signatures.size());
if(!images && !scan && !userData && !occupancyGrid)
{
UWARN("All requested data fields are false! Nothing loaded...");
return;
}
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
std::stringstream fields;
if(images)
{
fields << "image, depth, calibration";
if(scan || userData || occupancyGrid)
{
fields << ", ";
}
}
if(scan)
{
fields << "scan_info, scan";
if(userData || occupancyGrid)
{
fields << ", ";
}
}
if(userData)
{
fields << "user_data";
if(occupancyGrid)
{
fields << ", ";
}
}
if(occupancyGrid)
{
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
}
else
{
fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
}
}
query << "SELECT " << fields.str().c_str() << " "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data "
<< "FROM Data "
<< "INNER JOIN Node "
<< "ON Data.id = Node.id "
<< "WHERE Data.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data "
<< "FROM Image "
<< "INNER JOIN Node "
<< "on Image.id = Node.id "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data "
<< "FROM Image "
<< "INNER JOIN Node "
<< "on Image.id = Node.id "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
const void * data = 0;
int dataSize = 0;
int index = 0;
for(std::list<Signature*>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{
UASSERT(*iter != 0);
ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
index = 0;
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
Transform localTransform = Transform::getIdentity();
cv::Mat scanCompressed;
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.11.10") < 0 || images)
{
//Create the image
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
//Create the depth image
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
if(dataSize >= int(sizeof(int)*4))
{
const int * dataInt = (const int *)data;
int type = dataInt[3];
if(type == 0) // mono
{
CameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
models.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
}
else
{
UFATAL("Unknown calibration type %d", type);
}
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
else
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
UDEBUG("Loading calibration version >= 0.7.0");
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
UDEBUG("Loading calibration version < 0.7.0");
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
}
int laserScanMaxPts = 0;
float laserScanMinRange = 0.0f;
float laserScanMaxRange = 0.0f;
int laserScanFormat = 0;
float laserScanAngleMin = 0.0f;
float laserScanAngleMax = 0.0f;
float laserScanAngleInc = 0.0f;
Transform scanLocalTransform = Transform::getIdentity();
if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
{
// scan_info
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
UASSERT(dataSize == (int)((scanLocalTransform.size()+7)*sizeof(float)));
laserScanFormat = (int)dataFloat[0];
laserScanMinRange = dataFloat[1];
laserScanMaxRange = dataFloat[2];
laserScanAngleMin = dataFloat[3];
laserScanAngleMax = dataFloat[4];
laserScanAngleInc = dataFloat[5];
laserScanMaxPts = (int)dataFloat[6];
memcpy(scanLocalTransform.data(), dataFloat+7, scanLocalTransform.size()*sizeof(float));
}
else
{
if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((scanLocalTransform.size()+3)*sizeof(float)))
{
// new in 0.16.1
laserScanFormat = (int)dataFloat[2];
memcpy(scanLocalTransform.data(), dataFloat+3, scanLocalTransform.size()*sizeof(float));
}
else if(dataSize == (int)((scanLocalTransform.size()+2)*sizeof(float)))
{
memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
}
else
{
UFATAL("Unexpected size %d for laser scan info!", dataSize);
}
if(uStrNumCmp(_version, "0.15.2") < 0)
{
scanLocalTransform.normalizeRotation();
}
laserScanMaxPts = (int)dataFloat[0];
laserScanMaxRange = dataFloat[1];
}
}
}
else
{
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
if(uStrNumCmp(_version, "0.10.7") >= 0)
{
laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
}
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
if(dataSize>4 && data)
{
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
}
if(uStrNumCmp(_version, "0.11.10") < 0 || userData)
{
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
else
{
// compress data (set uncompressed data to signed to make difference with compressed type)
userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
}
}
}
}
// Occupancy grid
cv::Mat groundCellsCompressed;
cv::Mat obstacleCellsCompressed;
cv::Mat emptyCellsCompressed;
float cellSize = 0.0f;
cv::Point3f viewPoint;
if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
{
// ground
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
groundCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)groundCellsCompressed.data, data, dataSize);
}
// obstacle
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
obstacleCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
}
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
// empty
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)emptyCellsCompressed.data, data, dataSize);
}
}
cellSize = sqlite3_column_double(ppStmt, index++);
viewPoint.x = sqlite3_column_double(ppStmt, index++);
viewPoint.y = sqlite3_column_double(ppStmt, index++);
viewPoint.z = sqlite3_column_double(ppStmt, index++);
}
if(scan)
{
LaserScan laserScan;
if(laserScanAngleMin < laserScanAngleMax && laserScanAngleInc != 0.0f)
{
laserScan = LaserScan(scanCompressed, (LaserScan::Format)laserScanFormat, laserScanMinRange, laserScanMaxRange, laserScanAngleMin, laserScanAngleMax, laserScanAngleInc, scanLocalTransform);
}
else
{
laserScan = LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform);
}
(*iter)->sensorData().setLaserScan(laserScan);
}
if(images)
{
if(models.size())
{
(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
}
else
{
(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModel);
}
}
if(userData)
{
(*iter)->sensorData().setUserData(userDataCompressed);
}
if(occupancyGrid)
{
(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
bool DBDriverSqlite3::getCalibrationQuery(
int signatureId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const
{
bool found = false;
if(_ppDb && signatureId)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT calibration "
<< "FROM Data "
<< "WHERE id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT local_transform, fx, fy, cx, cy "
<< "FROM Depth "
<< "WHERE id = " << signatureId
<<";";
}
else
{
query << "SELECT local_transform, constant "
<< "FROM Depth "
<< "WHERE id = " << signatureId
<<";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
const void * data = 0;
int dataSize = 0;
Transform localTransform = Transform::getIdentity();
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
found = true;
int index = 0;
// calibration
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
localTransform.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
if(dataSize >= int(sizeof(int)*4))
{
const int * dataInt = (const int *)data;
int type = dataInt[3];
if(type == 0) // mono
{
CameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
models.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
}
else
{
UFATAL("Unknown calibration type %d", type);
}
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
else
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
UDEBUG("Loading calibration version >= 0.7.0");
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
UDEBUG("fx=%f fyOrBaseline=%f cx=%f cy=%f", fx, fyOrBaseline, cx, cy);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
UDEBUG("Loading calibration version < 0.7.0");
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return found;
}
bool DBDriverSqlite3::getLaserScanInfoQuery(
int signatureId,
LaserScan & info) const
{
bool found = false;
if(_ppDb && signatureId)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
query << "SELECT scan_info "
<< "FROM Data "
<< "WHERE id = " << signatureId
<<";";
}
else
{
return false;
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
const void * data = 0;
int dataSize = 0;
Transform localTransform = Transform::getIdentity();
int format = 0;
int maxPts = 0;
float minRange = 0.0f;
float maxRange = 0.0f;
float angleMin = 0.0f;
float angleMax = 0.0f;
float angleInc = 0.0f;
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
found = true;
int index = 0;
// scan_info
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
UASSERT(dataSize == (int)((localTransform.size()+7)*sizeof(float)));
format = (int)dataFloat[0];
minRange = dataFloat[1];
maxRange = dataFloat[2];
angleMin = dataFloat[3];
angleMax = dataFloat[4];
angleInc = dataFloat[5];
maxPts = (int)dataFloat[6];
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
}
else
{
if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (int)((localTransform.size()+3)*sizeof(float)))
{
// new in 0.16.1
format = (int)dataFloat[2];
memcpy(localTransform.data(), dataFloat+3, localTransform.size()*sizeof(float));
}
else if(dataSize == (int)((localTransform.size()+2)*sizeof(float)))
{
memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
}
else
{
UFATAL("Unexpected size %d for laser scan info!", dataSize);
}
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
maxPts = (int)dataFloat[0];
maxRange = dataFloat[1];
}
if(angleInc != 0.0f && angleMin < angleMax)
{
info = LaserScan(cv::Mat(), (LaserScan::Format)format, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else
{
info = LaserScan(cv::Mat(), maxPts, maxRange, (LaserScan::Format)format, localTransform);
}
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return found;
}
bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
Transform & pose,
int & mapId,
int & weight,
std::string & label,
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity,
GPS & gps,
EnvSensors & sensors) const
{
bool found = false;
if(_ppDb && signatureId)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps, env_sensors "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.14.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else
{
query << "SELECT pose, map_id, weight "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
found = true;
int index = 0;
data = sqlite3_column_blob(ppStmt, index); // pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
pose.normalizeRotation();
}
}
mapId = sqlite3_column_int(ppStmt, index++); // map id
weight = sqlite3_column_int(ppStmt, index++); // weight
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
const unsigned char * p = sqlite3_column_text(ppStmt, index++);
if(p)
{
label = reinterpret_cast<const char*>(p); // label
}
stamp = sqlite3_column_double(ppStmt, index++); // stamp
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
groundTruthPose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
velocity.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
{
memcpy(velocity.data(), data, dataSize);
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // gps
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == 6*sizeof(double) && data)
{
const double * dataDouble = (const double *)data;
gps = GPS(dataDouble[0], dataDouble[1], dataDouble[2], dataDouble[3], dataDouble[4], dataDouble[5]);
}
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(data)
{
UASSERT(dataSize%sizeof(double)==0 && (dataSize/sizeof(double))%3 == 0);
const double * dataDouble = (const double *)data;
int sensorsNum = (dataSize/sizeof(double))/3;
for(int i=0; i<sensorsNum;++i)
{
EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
}
}
}
}
}
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
return found;
}
void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT n.id "
"FROM Node AS n "
"WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
"ORDER BY n.id;";
}
else
{
query = "SELECT n.id "
"FROM Node AS n "
"WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
"ORDER BY n.id;";
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
}
}
void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT DISTINCT id "
<< "FROM Node ";
if(ignoreChildren)
{
query << "INNER JOIN Link ";
query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
query << "WHERE from_id != to_id "; // ignore self referring links
}
if(ignoreBadSignatures)
{
if(ignoreChildren)
{
query << "AND ";
}
else
{
query << "WHERE ";
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << " id in (select node_id from Feature) ";
}
else
{
query << " id in (select node_id from Map_Node_Word) ";
}
}
query << "ORDER BY id";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
}
}
void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
{
links.clear();
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.18.3") >= 0 && !withLandmarks)
{
query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link WHERE type!=" << Link::kLandmark << " ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
}
else
{
query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int fromId = -1;
int toId = -1;
int type = Link::kUndef;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
fromId = sqlite3_column_int(ppStmt, index++);
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{
UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
}
if(!ignoreNullLinks || !transform.isNull())
{
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
}
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
}
else
{
// neighbor is 0, loop closures are 1 and 2 (child)
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName) const
{
if(_ppDb)
{
UDEBUG("get last %s from table \"%s\"", fieldName.c_str(), tableName.c_str());
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT COALESCE(MAX(" << fieldName << "), " << id << ") " // In case the table is empty, return back input id
<< "FROM " << tableName
<< ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
id = sqlite3_column_int(ppStmt, 0); // Signature Id
rc = sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
UDEBUG("No result from the DB for table %s with field %s", tableName.c_str(), fieldName.c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
{
ni = 0;
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT count(word_id) "
<< "FROM Feature "
<< "WHERE node_id=" << nodeId << ";";
}
else
{
query << "SELECT count(word_id) "
<< "FROM Map_Node_Word "
<< "WHERE node_id=" << nodeId << ";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
ni = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
ULOGGER_ERROR("No result !?! from the DB, node=%d",nodeId);
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const
{
if(_ppDb && landmarkId < 0 && uStrNumCmp(_version, "0.18.3") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT from_id, type, information_matrix, transform, user_data FROM Link WHERE to_id=" << landmarkId <<"";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
int fromId = -1;
int linkType = -1;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
fromId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
cv::Mat userDataCompressed;
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
//transform
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
}
else if(dataSize)
{
UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, landmarkId);
}
if(linkType >= 0 && linkType != Link::kUndef)
{
nodes.insert(std::make_pair(fromId, Link(fromId, landmarkId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed)));
}
else
{
UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
linkType, fromId, landmarkId);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
}
}
void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
{
if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT id FROM Node WHERE label='" << label <<"'";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
id = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
}
}
void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
{
if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
int id = sqlite3_column_int(ppStmt, index++);
const unsigned char * p = sqlite3_column_text(ppStmt, index++);
if(p)
{
std::string label = reinterpret_cast<const char*>(p);
if(!label.empty())
{
labels.insert(std::make_pair(id, label));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
}
}
void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
{
weight = 0;
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT weight FROM node WHERE id = "
<< nodeId
<< ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
weight= sqlite3_column_int(ppStmt, 0); // weight
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
{
ULOGGER_DEBUG("count=%d", (int)ids.size());
if(_ppDb && ids.size())
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
unsigned int loaded = 0;
// Load nodes information
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.14.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label "
<< "FROM Node "
<< "WHERE id=?;";
}
else
{
query << "SELECT id, map_id, weight, pose "
<< "FROM Node "
<< "WHERE id=?;";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
//ULOGGER_DEBUG("Loading node %d...", *iter);
// bind id
rc = sqlite3_bind_int(ppStmt, 1, *iter);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int id = 0;
int mapId = 0;
double stamp = 0.0;
int weight = 0;
Transform pose;
Transform groundTruthPose;
std::vector<float> velocity;
std::vector<double> gps;
EnvSensors sensors;
const void * data = 0;
int dataSize = 0;
std::string label;
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
int index = 0;
id = sqlite3_column_int(ppStmt, index++); // Signature Id
mapId = sqlite3_column_int(ppStmt, index++); // Map Id
weight = sqlite3_column_int(ppStmt, index++); // weight
data = sqlite3_column_blob(ppStmt, index); // pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
pose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
stamp = sqlite3_column_double(ppStmt, index++); // stamp
const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
if(p)
{
label = reinterpret_cast<const char*>(p);
}
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
groundTruthPose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
velocity.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
{
memcpy(velocity.data(), data, dataSize);
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
gps.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // gps
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
{
memcpy(gps.data(), data, dataSize);
}
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // env_sensors
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(data)
{
UASSERT(dataSize%sizeof(double)==0 && (dataSize/sizeof(double))%3 == 0);
const double * dataDouble = (const double *)data;
int sensorsNum = (dataSize/sizeof(double))/3;
for(int i=0; i<sensorsNum;++i)
{
EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
}
}
}
}
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// create the node
if(id)
{
ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
Signature * s = new Signature(
id,
mapId,
weight,
stamp,
label,
pose,
groundTruthPose);
if(velocity.size() == 6)
{
s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(gps.size() == 6)
{
s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
}
s->sensorData().setEnvSensors(sensors);
s->setSaved(true);
nodes.push_back(s);
++loaded;
}
else
{
UERROR("Signature %d not found in database!", *iter);
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
// Prepare the query... Get the map from signature and visual words
std::stringstream query2;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Feature "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query2 << " ORDER BY word_id"; // Needed for fast insertion below
query2 << ";";
rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
int descriptorSize = 0;
const void * descriptor = 0;
int dRealSize = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, cv::Point3f> visualWords3;
std::multimap<int, cv::Mat> descriptors;
bool allWords3NaN = true;
cv::Point3f depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
visualWordId = sqlite3_column_int(ppStmt, index++);
kpt.pt.x = sqlite3_column_double(ppStmt, index++);
kpt.pt.y = sqlite3_column_double(ppStmt, index++);
kpt.size = sqlite3_column_int(ppStmt, index++);
kpt.angle = sqlite3_column_double(ppStmt, index++);
kpt.response = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.12.0") >= 0)
{
kpt.octave = sqlite3_column_int(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.x = nanFloat;
++index;
}
else
{
depth.x = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.y = nanFloat;
++index;
}
else
{
depth.y = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.z = nanFloat;
++index;
}
else
{
depth.z = sqlite3_column_double(ppStmt, index++);
}
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
if(allWords3NaN && util3d::isFinite(depth))
{
allWords3NaN = false;
}
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(descriptor && descriptorSize>0 && dRealSize>0)
{
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(visualWords.size()==0)
{
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
(*iter)->setWords(visualWords);
if(!allWords3NaN)
{
(*iter)->setWords3(visualWords3);
}
(*iter)->setWordsDescriptors(descriptors);
ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
this->loadLinksQuery(nodes);
ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
(*iter)->setModified(false);
}
// load calibrations
if(nodes.size() && uStrNumCmp(_version, "0.10.0") >= 0)
{
std::stringstream query3;
query3 << "SELECT calibration "
"FROM Data "
"WHERE id = ? ";
rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
int index=0;
const void * data = 0;
int dataSize = 0;
Transform localTransform;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
// calibration
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, [width,height], local_transform] (5or7+12)*float
if(dataSize > 0 && data)
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
if(dataSize >= int(sizeof(int)*4))
{
const int * dataInt = (const int *)data;
int type = dataInt[3];
if(type == 0) // mono
{
CameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
models.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
}
else
{
UFATAL("Unknown calibration type %d", type);
}
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
else
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform,
0,
cv::Size(dataFloat[i+4], dataFloat[i+5])));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5], dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes, db version=%s)", dataSize, _version.c_str());
}
}
(*iter)->sensorData().setCameraModels(models);
(*iter)->sensorData().setStereoCameraModel(stereoModel);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
}
if(ids.size() != loaded)
{
UERROR("Some signatures not found in database");
}
}
}
void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
{
ULOGGER_DEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
std::list<int> ids;
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT n.id "
"FROM Node AS n "
"WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
"ORDER BY n.id;";
}
else
{
query = "SELECT n.id "
"FROM Node AS n "
"WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
"ORDER BY n.id;";
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
ids.push_back(sqlite3_column_int(ppStmt, 0)); // Signature id
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Loading %d signatures...", ids.size());
this->loadSignaturesQuery(ids, nodes);
ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
}
}
void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb && dictionary)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
std::list<VisualWord *> visualWords;
// Get the visual words
query << "SELECT id, descriptor_size, descriptor FROM Word ";
if(lastStateOnly)
{
if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
}
else
{
query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
}
}
query << "ORDER BY id;";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
int id = 0;
int descriptorSize = 0;
const void * descriptor = 0;
int dRealSize = 0;
rc = sqlite3_step(ppStmt);
int count = 0;
while(rc == SQLITE_ROW)
{
int index=0;
id = sqlite3_column_int(ppStmt, index++); // VisualWord Id
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
dictionary->addWord(vw);
if(++count % 5000 == 0)
{
ULOGGER_DEBUG("Loaded %d words...", count);
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Get Last word id
getLastWordId(id);
dictionary->setLastWordId(id);
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
{
ULOGGER_DEBUG("size=%d", wordIds.size());
if(_ppDb && wordIds.size())
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
std::set<int> loaded;
// Get the map from signature and visual words
query << "SELECT vw.descriptor_size, vw.descriptor "
"FROM Word as vw "
"WHERE vw.id = ?;";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int descriptorSize;
const void * descriptor;
int dRealSize;
for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
{
// bind id
rc = sqlite3_bind_int(ppStmt, 1, *iter);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
int index = 0;
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(*iter, d);
if(vw)
{
vw->setSaved(true);
}
vws.push_back(vw);
loaded.insert(loaded.end(), *iter);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
if(wordIds.size() != loaded.size())
{
for(std::set<int>::const_iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
{
if(loaded.find(*iter) == loaded.end())
{
UDEBUG("Not found word %d", *iter);
}
}
UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
}
}
}
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::multimap<int, Link> & links,
Link::Type typeIn) const
{
links.clear();
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, transform, variance FROM Link ";
}
else
{
query << "SELECT to_id, type, transform FROM Link ";
}
query << "WHERE from_id = " << signatureId;
if(typeIn != Link::kUndef)
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << " AND type = " << typeIn;
}
else if(typeIn == Link::kNeighbor)
{
query << " AND type = 0";
}
else if(typeIn > Link::kNeighbor)
{
query << " AND type > 0";
}
}
if(uStrNumCmp(_version, "0.18.3") >= 0 && (typeIn != Link::kAllWithLandmarks && typeIn != Link::kLandmark))
{
query << " AND type != " << Link::kLandmark;
}
query << " ORDER BY to_id";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int toId = -1;
int type = Link::kUndef;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{
UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
}
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
}
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
}
else
{
// neighbor is 0, loop closures are 1
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(links.size() == 0)
{
//UERROR("No links loaded from signature %d", signatureId);
}
}
}
void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, variance, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else
{
query << "SELECT to_id, type, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int toId = -1;
int linkType = -1;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
cv::Mat userDataCompressed;
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
}
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
}
//transform
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{
UERROR("Error while loading link transform from %d to %d! Setting to null...", (*iter)->id(), toId);
}
if(linkType >= 0 && linkType != Link::kUndef)
{
if(linkType == Link::kLandmark)
{
(*iter)->addLandmark(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
}
else
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
}
}
}
else
{
UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
linkType, (*iter)->id(), toId);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// add links
(*iter)->addLinks(links);
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
{
UDEBUG("nodes = %d", nodes.size());
if(_ppDb && nodes.size())
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
Signature * s = 0;
std::string query;
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
if(updateTimestamp)
{
query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
}
else
{
query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
}
}
else
{
if(updateTimestamp)
{
query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
}
else
{
query = "UPDATE Node SET weight=? WHERE id=?;";
}
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator i=nodes.begin(); i!=nodes.end(); ++i)
{
s = *i;
int index = 1;
if(s)
{
rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
if(s->getLabel().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
rc = sqlite3_bind_int(ppStmt, index++, s->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
// Update links part1
if(uStrNumCmp(_version, "0.18.3") >= 0)
{
query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
}
else
{
query = uFormat("DELETE FROM Link WHERE from_id=?;");
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isLinksModified())
{
rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Update links part2
query = queryStepLink();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isLinksModified())
{
// Save links
const std::multimap<int, Link> & links = (*j)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Update Neighbors Time=%fs", timer.ticks());
// Update word references
query = queryStepWordsChanged();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->getWordsChanged().size())
{
const std::map<int, int> & wordsChanged = (*j)->getWordsChanged();
for(std::map<int, int>::const_iterator iter=wordsChanged.begin(); iter!=wordsChanged.end(); ++iter)
{
stepWordsChanged(ppStmt, (*j)->id(), iter->first, iter->second);
}
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("signatures update=%fs", timer.ticks());
}
}
void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
{
if(_ppDb && words.size() && updateTimestamp)
{
// Only timestamp update is done here, so don't enter this if at all if false
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
VisualWord * w = 0;
std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
{
w = *i;
int index = 1;
UASSERT(w);
rc = sqlite3_bind_int(ppStmt, index++, w->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
{
UDEBUG("");
if(_ppDb && signatures.size())
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Signature table
std::string query = queryStepNode();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
_memoryUsedEstimate += (*i)->getMemoryUsed();
// raw data are not kept in database
_memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
stepNode(ppStmt, *i);
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Create new entries in table Link
query = queryStepLink();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save links
const std::multimap<int, Link> & links = (*jter)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
if(uStrNumCmp(_version, "0.18.3") >= 0)
{
// Save landmarks
const std::map<int, Link> & links = (*jter)->getLandmarks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Create new entries in table Feature
query = queryStepKeypoint();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
{
cv::Point3f pt(nanFloat,nanFloat,nanFloat);
if(p!=(*i)->getWords3().end())
{
UASSERT(w->first == p->first); // must be same id!
pt = p->second;
++p;
}
cv::Mat descriptor;
if(d!=(*i)->getWordsDescriptors().end())
{
UASSERT(w->first == d->first); // must be same id!
descriptor = d->second;
++d;
}
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
// Add SensorData
query = queryStepSensorData();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
if(!(*i)->sensorData().imageCompressed().empty() ||
!(*i)->sensorData().depthOrRightCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().isEmpty() ||
!(*i)->sensorData().userDataCompressed().empty() ||
!(*i)->sensorData().cameraModels().size() ||
!(*i)->sensorData().stereoCameraModel().isValidForProjection())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepSensorData(ppStmt, (*i)->sensorData());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
else
{
// Add images
query = queryStepImage();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
if(!(*i)->sensorData().imageCompressed().empty())
{
stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Add depths
query = queryStepDepth();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
//metric
if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().isEmpty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepDepth(ppStmt, (*i)->sensorData());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
{
UDEBUG("visualWords size=%d", words.size());
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Create new entries in table Map_SS_VW
if(words.size()>0)
{
query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
const VisualWord * w = *iter;
UASSERT(w);
if(!w->isSaved())
{
rc = sqlite3_bind_int(ppStmt, 1, w->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
if(w->getDescriptor().type() == CV_32F)
{
// CV_32F
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
}
else
{
// CV_8U
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::addLinkQuery(const Link & link) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create new entries in table Link
std::string query = queryStepLink();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save link
stepLink(ppStmt, link);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::updateLinkQuery(const Link & link) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create new entries in table Link
std::string query = queryStepLinkUpdate();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save link
stepLink(ppStmt, link);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::updateOccupancyGridQuery(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create query
std::string query = queryStepOccupancyGridUpdate();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save occupancy grid
stepOccupancyGridUpdate(ppStmt,
nodeId,
ground,
obstacles,
empty,
cellSize,
viewpoint);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::updateDepthImageQuery(
int nodeId,
const cv::Mat & image) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create query
std::string query = queryStepDepthUpdate();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save depth
stepDepthUpdate(ppStmt,
nodeId,
image);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::updateLaserScanQuery(
int nodeId,
const LaserScan & scan) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create query
std::string query = queryStepScanUpdate();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save depth
stepScanUpdate(ppStmt,
nodeId,
scan);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
{
UDEBUG("Ref ID = %d", statistics.refImageId());
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create query
if(uStrNumCmp(this->getDatabaseVersion(), "0.11.11") >= 0)
{
std::string param = Statistics::serializeData(statistics.data());
if(param.size() && statistics.refImageId()>0)
{
std::string query;
if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
{
query = "INSERT INTO Statistics(id, stamp, data, wm_state) values(?,?,?,?);";
}
else
{
query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, statistics.refImageId());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, statistics.stamp());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
cv::Mat compressedParam;
if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
{
compressedParam = compressString(param);
rc = sqlite3_bind_blob(ppStmt, index++, compressedParam.data, compressedParam.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
cv::Mat compressedWmState;
if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
{
if(!statistics.wmState().empty())
{
compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
}
}
void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
cv::Mat compressedImage;
if(image.empty())
{
rc = sqlite3_bind_null(ppStmt, index);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// compress
if(image.rows == 1 && image.type() == CV_8UC1)
{
// already compressed
compressedImage = image;
}
else
{
compressedImage = compressImage2(image, ".jpg");
}
rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
{
UDEBUG("");
cv::Mat image;
if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT preview_image "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
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());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_cloud
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
}
UDEBUG("Image=%dx%d", image.cols, image.rows);
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return image;
}
void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
// opt ids and poses
cv::Mat compressedIds;
cv::Mat compressedPoses;
if(poses.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
std::vector<int> serializedIds(poses.size());
std::vector<float> serializedPoses(poses.size()*12);
int i=0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
serializedIds[i] = iter->first;
memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
++i;
}
compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(lastlocalizationPose.isNull())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
{
UDEBUG("");
std::map<int, Transform> poses;
if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT opt_ids, opt_poses, opt_last_localization "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
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());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_poses
cv::Mat serializedIds;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedIds=%d", serializedIds.cols);
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedPoses=%d", serializedPoses.cols);
UASSERT(serializedIds.cols == serializedPoses.cols/12);
UASSERT(serializedPoses.type() == CV_32FC1);
UASSERT(serializedIds.type() == CV_32SC1);
for(int i=0; i<serializedIds.cols; ++i)
{
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),
serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
poses.insert(std::make_pair(serializedIds.at<int>(i), t));
UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
}
}
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(lastlocalizationPose)
{
if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
{
memcpy(lastlocalizationPose->data(), data, dataSize);
}
UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return poses;
}
void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
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());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
// opt ids and poses
cv::Mat compressedMap;
if(map.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
compressedMap = compressData2(map);
rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_double(ppStmt, index++, xMin);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, yMin);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, cellSize);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
{
UDEBUG("");
cv::Mat map;
if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
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());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_map
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("map=%d/%d", map.cols, map.rows);
}
xMin = sqlite3_column_double(ppStmt, index++);
UDEBUG("xMin=%f", xMin);
yMin = sqlite3_column_double(ppStmt, index++);
UDEBUG("yMin=%f", yMin);
cellSize = sqlite3_column_double(ppStmt, index++);
UDEBUG("cellSize=%f", cellSize);
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return map;
}
void DBDriverSqlite3::saveOptimizedMeshQuery(
const cv::Mat & cloud,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
#else
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
#endif
const cv::Mat & textures) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
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());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(cloud.empty())
{
// set all fields to null
for(int i=1; i<=5; ++i)
{
rc = sqlite3_bind_null(ppStmt, i);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
int index = 1;
// compress and save cloud
cv::Mat compressedCloud;
if(cloud.rows == 1 && cloud.type() == CV_8UC1)
{
// already compressed
compressedCloud = cloud;
}
else
{
UDEBUG("Cloud points=%d", cloud.cols);
compressedCloud = compressData2(cloud);
}
UDEBUG("Cloud compressed bytes=%d", compressedCloud.cols);
rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// opt ids and poses
cv::Mat compressedPolygons;
cv::Mat compressedTexCoords;
cv::Mat compressedTextures;
// polygons
if(polygons.empty())
{
//polygon size
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// polygons
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// tex_coords
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// materials
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
std::vector<int> serializedPolygons;
std::vector<float> serializedTexCoords;
int polygonSize = 0;
int totalPolygonIndices = 0;
UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
for(unsigned int t=0; t<polygons.size(); ++t)
{
UDEBUG("t=%d, polygons=%d", t, (int)polygons[t].size());
unsigned int materialPolygonIndices = 0;
for(unsigned int p=0; p<polygons[t].size(); ++p)
{
if(polygonSize == 0)
{
UASSERT(polygons[t][p].size());
polygonSize = polygons[t][p].size();
}
else
{
UASSERT(polygonSize == (int)polygons[t][p].size());
}
materialPolygonIndices += polygons[t][p].size();
}
totalPolygonIndices += materialPolygonIndices;
if(!texCoords.empty())
{
UASSERT(materialPolygonIndices == texCoords[t].size());
}
}
UASSERT(totalPolygonIndices>0);
serializedPolygons.resize(totalPolygonIndices+polygons.size());
if(!texCoords.empty())
{
serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
}
int oi=0;
int ci=0;
for(unsigned int t=0; t<polygons.size(); ++t)
{
serializedPolygons[oi++] = polygons[t].size();
if(!texCoords.empty())
{
serializedTexCoords[ci++] = texCoords[t].size();
}
for(unsigned int p=0; p<polygons[t].size(); ++p)
{
int texIndex = p*polygonSize;
for(unsigned int i=0; i<polygons[t][p].size(); ++i)
{
serializedPolygons[oi++] = polygons[t][p][i];
if(!texCoords.empty())
{
serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
}
}
}
}
UDEBUG("serializedPolygons=%d", (int)serializedPolygons.size());
compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
// polygon size
rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// tex coords
if(texCoords.empty())
{
// tex coords
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// materials
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
UDEBUG("serializedTexCoords=%d", (int)serializedTexCoords.size());
compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
if(textures.rows == 1 && textures.type() == CV_8UC1)
{
//already compressed
compressedTextures = textures;
}
else
{
compressedTextures = compressImage2(textures, ".jpg");
}
rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
#else
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
#endif
cv::Mat * textures) const
{
UDEBUG("");
cv::Mat cloud;
if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
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());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_cloud
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
}
UDEBUG("Cloud=%d points", cloud.cols);
//opt_polygons_size
int polygonSize = sqlite3_column_int(ppStmt, index++);
UDEBUG("polygonSize=%d", polygonSize);
//opt_polygons
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
UASSERT(polygonSize > 0);
if(polygons)
{
cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
UASSERT(serializedPolygons.total());
for(int t=0; t<serializedPolygons.cols; ++t)
{
UASSERT(serializedPolygons.at<int>(t) > 0);
std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
++t;
UASSERT(t < serializedPolygons.cols);
UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
for(int p=0; p<(int)materialPolygons.size(); ++p)
{
for(int i=0; i<polygonSize; ++i)
{
materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
}
}
t+=materialPolygons.size()*polygonSize;
polygons->push_back(materialPolygons);
}
}
//opt_tex_coords
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
if(texCoords)
{
cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
UASSERT(serializedTexCoords.total());
for(int t=0; t<serializedTexCoords.cols; ++t)
{
UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
#else
std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
#endif
++t;
UASSERT(t < serializedTexCoords.cols);
UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
for(int p=0; p<(int)materialtexCoords.size(); ++p)
{
materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
}
t+=materialtexCoords.size()*2;
texCoords->push_back(materialtexCoords);
}
}
//opt_tex_materials
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
if(textures)
{
*textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("textures=%dx%d", textures->cols, textures->rows);
}
}
}
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return cloud;
}
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors) VALUES(?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.14.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps) VALUES(?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.1") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
}
return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
}
void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
{
UDEBUG("Save node %d", s->id());
if(!ppStmt || !s)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, s->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, s->mapId());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(s->getLabel().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
std::vector<double> gps;
std::vector<double> envSensors;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
// ignore user_data
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
if(s->getVelocity().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
if(s->sensorData().gps().stamp() <= 0.0)
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
gps.resize(6,0.0);
gps[0] = s->sensorData().gps().stamp();
gps[1] = s->sensorData().gps().longitude();
gps[2] = s->sensorData().gps().latitude();
gps[3] = s->sensorData().gps().altitude();
gps[4] = s->sensorData().gps().error();
gps[5] = s->sensorData().gps().bearing();
rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
const EnvSensors & sensors = s->sensorData().envSensors();
if(sensors.size() == 0)
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
envSensors.resize(sensors.size()*3,0.0);
int j=0;
for(std::map<EnvSensor::Type, EnvSensor>::const_iterator iter=sensors.begin(); iter!=sensors.end(); ++iter, j+=3)
{
envSensors[j] = (double)iter->second.type();
envSensors[j+1] = iter->second.value();
envSensors[j+2] = iter->second.stamp();
}
rc = sqlite3_bind_blob(ppStmt, index++, envSensors.data(), envSensors.size()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
}
}
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
if(s->sensorData().userDataCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepImage() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
return "INSERT INTO Image(id, data) VALUES(?,?);";
}
void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
int id,
const cv::Mat & imageBytes) const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, id);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(!imageBytes.empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepDepth() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
}
else
{
return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
}
}
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
UDEBUG("Save depth %d (size=%d) depth2d = %d",
sensorData.id(),
(int)sensorData.depthOrRightCompressed().cols,
sensorData.laserScanCompressed().size());
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(!sensorData.depthOrRightCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float fx=0, fyOrBaseline=0, cx=0, cy=0;
Transform localTransform = Transform::getIdentity();
if(sensorData.cameraModels().size())
{
UASSERT_MSG(sensorData.cameraModels().size() == 1,
uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
fx = sensorData.cameraModels()[0].fx();
fyOrBaseline = sensorData.cameraModels()[0].fy();
cx = sensorData.cameraModels()[0].cx();
cy = sensorData.cameraModels()[0].cy();
localTransform = sensorData.cameraModels()[0].localTransform();
}
else if(sensorData.stereoCameraModel().isValidForProjection())
{
fx = sensorData.stereoCameraModel().left().fx();
fyOrBaseline = sensorData.stereoCameraModel().baseline();
cx = sensorData.stereoCameraModel().left().cx();
cy = sensorData.stereoCameraModel().left().cy();
localTransform = sensorData.stereoCameraModel().left().localTransform();
}
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, cx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, cy);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(!sensorData.laserScanCompressed().isEmpty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, (int)sensorData.laserScanCompressed().size(), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepDepthUpdate() const
{
if(uStrNumCmp(_version, "0.10.0") < 0)
{
return "UPDATE Depth SET data=? WHERE id=?;";
}
else
{
return "UPDATE Data SET depth=? WHERE id=?;";
}
}
void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
cv::Mat imageCompressed;
if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
{
// compress
imageCompressed = compressImage2(image, ".png");
}
else
{
imageCompressed = image;
}
if(!imageCompressed.empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, imageCompressed.data, (int)imageCompressed.cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//id
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepScanUpdate() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
return "UPDATE Data SET scan_info=?, scan=? WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
return "UPDATE Data SET scan_max_pts=?, scan_max_range=?, scan=? WHERE id=?;";
}
else
{
return "UPDATE Data SET scan_max_pts=? scan=? WHERE id=?;";
}
}
void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & scan) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
std::vector<float> scanInfo;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
if(scan.maxPoints() > 0 ||
scan.rangeMax() > 0 ||
(uStrNumCmp(_version, "0.16.1")>=0 && scan.format() != LaserScan::kUnknown) ||
(!scan.localTransform().isNull() && !scan.localTransform().isIdentity()))
{
if(uStrNumCmp(_version, "0.16.1") >=0)
{
if(uStrNumCmp(_version, "0.18.0") >=0)
{
scanInfo.resize(7 + Transform().size());
scanInfo[0] = scan.format();
scanInfo[1] = scan.rangeMin();
scanInfo[2] = scan.rangeMax();
scanInfo[3] = scan.angleMin();
scanInfo[4] = scan.angleMax();
scanInfo[5] = scan.angleIncrement();
scanInfo[6] = scan.maxPoints(); // only for backward compatibility
const Transform & localTransform = scan.localTransform();
memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
else
{
scanInfo.resize(3 + Transform().size());
scanInfo[0] = scan.maxPoints();
scanInfo[1] = scan.rangeMax();
scanInfo[2] = scan.format();
const Transform & localTransform = scan.localTransform();
memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else
{
scanInfo.resize(2 + Transform().size());
scanInfo[0] = scan.maxPoints();
scanInfo[1] = scan.rangeMax();
const Transform & localTransform = scan.localTransform();
memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
}
}
if(scanInfo.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, scan.maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// scan_max_range
if(uStrNumCmp(_version, "0.10.7") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, scan.rangeMax());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// scan
cv::Mat scanCompressed;
if(scan.isCompressed())
{
scanCompressed = scan.data();
}
else
{
scanCompressed = compressData2(scan.data());
}
if(!scanCompressed.empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, scanCompressed.data, scanCompressed.total(), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//id
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
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(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.11.10") >= 0)
{
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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.1") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
}
else
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
}
}
void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
const SensorData & sensorData) const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
sensorData.id(),
(int)sensorData.imageCompressed().cols,
(int)sensorData.depthOrRightCompressed().cols,
sensorData.laserScanCompressed().size());
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
// id
rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// image
if(!sensorData.imageCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// depth or right image
if(!sensorData.depthOrRightCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// calibration
std::vector<unsigned char> calibrationData;
std::vector<float> calibration;
// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
UASSERT(sensorData.cameraModels()[i].isValidForProjection());
std::vector<unsigned char> data = sensorData.cameraModels()[i].serialize();
UASSERT(!data.empty());
unsigned int oldSize = calibrationData.size();
calibrationData.resize(calibrationData.size() + data.size());
memcpy(calibrationData.data()+oldSize, data.data(), data.size());
}
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
UASSERT(sensorData.cameraModels()[i].isValidForProjection());
const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else
{
calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
UASSERT(sensorData.cameraModels()[i].isValidForProjection());
const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
}
}
}
else if(sensorData.stereoCameraModel().isValidForProjection())
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
calibrationData = sensorData.stereoCameraModel().serialize();
UASSERT(!calibrationData.empty());
}
else
{
const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
calibration.resize(7+localTransform.size());
calibration[0] = sensorData.stereoCameraModel().left().fx();
calibration[1] = sensorData.stereoCameraModel().left().fy();
calibration[2] = sensorData.stereoCameraModel().left().cx();
calibration[3] = sensorData.stereoCameraModel().left().cy();
calibration[4] = sensorData.stereoCameraModel().baseline();
calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
}
if(calibrationData.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
}
else if(calibration.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
std::vector<float> scanInfo;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
if(sensorData.laserScanCompressed().maxPoints() > 0 ||
sensorData.laserScanCompressed().rangeMax() > 0 ||
(uStrNumCmp(_version, "0.16.1")>=0 && sensorData.laserScanCompressed().format() != LaserScan::kUnknown) ||
(!sensorData.laserScanCompressed().localTransform().isNull() && !sensorData.laserScanCompressed().localTransform().isIdentity()))
{
if(uStrNumCmp(_version, "0.16.1") >=0)
{
if(uStrNumCmp(_version, "0.18.0") >=0)
{
scanInfo.resize(7 + Transform().size());
scanInfo[0] = sensorData.laserScanCompressed().format();
scanInfo[1] = sensorData.laserScanCompressed().rangeMin();
scanInfo[2] = sensorData.laserScanCompressed().rangeMax();
scanInfo[3] = sensorData.laserScanCompressed().angleMin();
scanInfo[4] = sensorData.laserScanCompressed().angleMax();
scanInfo[5] = sensorData.laserScanCompressed().angleIncrement();
scanInfo[6] = sensorData.laserScanCompressed().maxPoints(); // only for backward compatibility
const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
else
{
scanInfo.resize(3 + Transform().size());
scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
scanInfo[2] = sensorData.laserScanCompressed().format();
const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else
{
scanInfo.resize(2 + Transform().size());
scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
scanInfo[1] = sensorData.laserScanCompressed().rangeMax();
const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
}
}
if(scanInfo.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// scan_max_range
if(uStrNumCmp(_version, "0.10.7") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanCompressed().rangeMax());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// scan
if(!sensorData.laserScanCompressed().isEmpty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, sensorData.laserScanCompressed().size(), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
// user_data
if(!sensorData.userDataCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
//ground_cells
if(sensorData.gridGroundCellsCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// compress
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//obstacle_cells
if(sensorData.gridObstacleCellsCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
//empty_cells
if(sensorData.gridEmptyCellsCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//cell_size
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//view_point
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepLinkUpdate() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
}
else
{
return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
}
}
std::string DBDriverSqlite3::queryStepLink() const
{
// from_id, to_id are at the end to match the update query above
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
}
else
{
return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
}
}
void DBDriverSqlite3::stepLink(
sqlite3_stmt * ppStmt,
const Link & link) const
{
if(!ppStmt)
{
UFATAL("");
}
UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
// Don't save virtual links
if(link.type()==Link::kVirtualClosure)
{
UDEBUG("Virtual link ignored....");
return;
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, link.type());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
// information_matrix
rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
// user_data
if(!link.userDataCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_int(ppStmt, index++, link.from());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, link.to());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepWordsChanged() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
}
else
{
return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
}
}
void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, newWordId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, oldWordId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepKeypoint() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
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(?,?,?,?,?,?,?,?,?,?,?,?);";
}
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
}
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
int nodeId,
int wordId,
const cv::KeyPoint & kp,
const cv::Point3f & pt,
const cv::Mat & descriptor) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, wordId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, kp.pt.x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, kp.pt.y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, kp.size);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, kp.angle);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, kp.response);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.12.0") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, kp.octave);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uIsFinite(pt.x))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uIsFinite(pt.y))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uIsFinite(pt.z))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//descriptor
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
if(descriptor.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
}
else
{
if(descriptor.type() == CV_32F)
{
// CV_32F
rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
}
else
{
// CV_8U
rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
}
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
{
UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
}
return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
}
void DBDriverSqlite3::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
{
UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
nodeId,
ground.cols,
obstacles.cols,
empty.cols,
cellSize,
viewpoint.x,
viewpoint.y,
viewpoint.z);
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
//ground_cells
if(ground.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// compress
rc = sqlite3_bind_blob(ppStmt, index++, ground.data, ground.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//obstacle_cells
if(obstacles.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, obstacles.data, obstacles.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
//empty_cells
if(empty.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//cell_size
rc = sqlite3_bind_double(ppStmt, index++, cellSize);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//view_point
rc = sqlite3_bind_double(ppStmt, index++, viewpoint.x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, viewpoint.y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, viewpoint.z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// id
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
} // namespace rtabmap
Reference in New Issue View Git Blame Copy Permalink