i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Loading time optimization (#1569)

Browse Source 
* On init, rebuild the dictionnary only once

* Fixed first dictionary update to avoid rebuilding multiple times. Commented some very verbose debug logs (should create a new level: UVERBOSE or UTRACE)

* bump version 0.23: added parameters "Mem/LoadVisualLocalFeaturesOnInit", "Mem/FlannIndexSaved", "Kp/SerializeWithChecksum".

* Renamed Mem/FlannIndexSaved to Kp/FlannIndexSaved. Update UI Preferences with new parameters.

* commented some very verbose debug logs

* Warn flann index serialization not implemented on windows

* Warn flann index deserialization not implemented on windows

* Removing some verbose logs

* missing header (win32)

* Added GlobalMap::fullUpdateNeeded() function

* Adding log

* Save flann index even if links changed

* log time to serialize flann index

* Make dictionary modified only after we update

---------

Co-authored-by: Mathieu Labbe <mathieu@robust.ai>
This commit is contained in:
matlabbe
2025-09-03 16:55:54 -07:00
committed by GitHub
parent 349580299c
commit 4603f09389
35 changed files with 1811 additions and 791 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/CMakeLists.txt
View File

@@ -816,6 +816,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
SET(RESOURCES
${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_22_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql

2
corelib/src/CameraModel.cpp
View File

@@ -554,7 +554,7 @@ unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int d
int iR = 8;
int iP = 9;
int iL = 10;
UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
//UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
unsigned int requiredDataSize = sizeof(int)*headerSize +
sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
sizeof(float)*header[iL];

63
corelib/src/DBDriver.cpp
View File

@@ -383,7 +383,7 @@ void DBDriver::asyncSave(Signature * s)
{
if(s)
{
UDEBUG("s=%d", s->id());
//UDEBUG("s=%d", s->id());
_trashesMutex.lock();
{
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
@@ -531,17 +531,17 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
{
_dbSafeAccessMutex.lock();
this->loadQuery(dictionary, lastStateOnly);
_dbSafeAccessMutex.unlock();
}
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
void DBDriver::loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly) const
{
_dbSafeAccessMutex.lock();
this->loadLastNodesQuery(signatures);
this->loadLastNodesQuery(signatures, loadWordIdsOnly);
_dbSafeAccessMutex.unlock();
}
@@ -564,7 +564,8 @@ Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash)
}
void DBDriver::loadSignatures(const std::list<int> & signIds,
std::list<Signature *> & signatures,
std::set<int> * loadedFromTrash)
std::set<int> * loadedFromTrash,
bool loadWordIdsOnly)
{
UDEBUG("");
// look up in the trash before the database
@@ -609,7 +610,7 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
if(ids.size())
{
_dbSafeAccessMutex.lock();
this->loadSignaturesQuery(ids, signatures);
this->loadSignaturesQuery(ids, signatures, loadWordIdsOnly);
_dbSafeAccessMutex.unlock();
}
}
@@ -656,10 +657,10 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
}
}
void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const
void DBDriver::loadNodeData(Signature & signature, bool images, bool scan, bool userData, bool occupancyGrid) const
{
std::list<Signature *> signatures;
signatures.push_back(signature);
signatures.push_back(&signature);
this->loadNodeData(signatures, images, scan, userData, occupancyGrid);
}
@@ -823,6 +824,45 @@ bool DBDriver::getNodeInfo(
return found;
}
void DBDriver::getLocalFeatures(
int signatureId,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & points,
cv::Mat & descriptors) const
{
bool found = false;
// look in the trash
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
UASSERT(s != 0);
found = true;
if(!s->getWords().empty())
{
words = s->getWords();
if(s->getWordsKpts().empty()){
found = false; // Force checking the database in case the local features were not loaded in RAM
}
else
{
words = s->getWords();
keypoints = s->getWordsKpts();
points = s->getWords3();
descriptors = s->getWordsDescriptors().clone();
}
}
}
_trashesMutex.unlock();
if(!found)
{
UScopeMutex lock(_dbSafeAccessMutex);
getLocalFeaturesQuery(signatureId, words, keypoints, points, descriptors);
}
}
void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
{
bool found = false;
@@ -1287,6 +1327,13 @@ cv::Mat DBDriver::loadOptimizedMesh(
return cloud;
}
void DBDriver::saveFlannIndex(const std::vector<unsigned char> & indexData) const
{
_dbSafeAccessMutex.lock();
saveFlannIndexQuery(indexData);
_dbSafeAccessMutex.unlock();
}
void DBDriver::generateGraph(
const std::string & fileName,
const std::set<int> & idsInput,

565
corelib/src/DBDriverSqlite3.cpp
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include "DatabaseSchema_sql.h"
#include "DatabaseSchema_0_22_0_sql.h"
#include "DatabaseSchema_0_20_0_sql.h"
#include "DatabaseSchema_0_18_3_sql.h"
#include "DatabaseSchema_0_18_0_sql.h"
@@ -404,6 +405,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
schemas.push_back(std::make_pair("0.22.0", DATABASESCHEMA_0_22_0_SQL));
schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
for(size_t i=0; i<schemas.size(); ++i)
{
@@ -1296,8 +1298,8 @@ std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery(
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
(int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
//UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
// (int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
if(!images && !scan && !userData && !occupancyGrid)
{
@@ -1445,7 +1447,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
UASSERT(*iter != 0);
ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
//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());
@@ -1874,7 +1876,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
// 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());
//ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
@@ -2391,6 +2393,23 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
return found;
}
void DBDriverSqlite3::getLocalFeaturesQuery(
int signatureId,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & points,
cv::Mat & descriptors) const
{
Signature s(signatureId);
std::list<Signature *> ids;
ids.push_back(&s);
this->loadWordsQuery(ids);
words = ids.front()->getWords();
keypoints = ids.front()->getWordsKpts();
points = ids.front()->getWords3();
descriptors = ids.front()->getWordsDescriptors().clone();
}
void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
{
if(_ppDb)
@@ -2987,7 +3006,7 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
}
//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
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes, bool loadWordIdsOnly) const
{
ULOGGER_DEBUG("count=%d", (int)ids.size());
if(_ppDb && ids.size())
@@ -3151,7 +3170,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// create the node
if(id)
{
ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
//ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
Signature * s = new Signature(
id,
mapId,
@@ -3190,175 +3209,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
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 = ? ";
UDEBUG("Loading local features (ids only=%s)....", loadWordIdsOnly?"true":"false");
if(loadWordIdsOnly) {
this->loadWordIdsQuery(nodes);
}
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 {
this->loadWordsQuery(nodes);
}
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, int> visualWords;
std::vector<cv::KeyPoint> visualWordsKpts;
std::vector<cv::Point3f> visualWords3;
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++);
}
visualWordsKpts.push_back(kpt);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
visualWords3.push_back(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.push_back(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
{
if(allWords3NaN)
{
visualWords3.clear();
}
(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, 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.rows, (*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());
UDEBUG("Loading local features.... done! (in %f s)", timer.ticks());
this->loadLinksQuery(nodes);
ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
ULOGGER_DEBUG("Time loading links=%fs", timer.ticks());
for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
@@ -3626,7 +3487,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
}
void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool loadWordIdsOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb)
@@ -3672,15 +3533,15 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
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);
this->loadSignaturesQuery(ids, nodes, loadWordIdsOnly);
ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
}
}
void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb && dictionary)
if(_ppDb)
{
std::string type;
UTimer timer;
@@ -3743,11 +3604,11 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
dictionary->addWord(vw);
dictionary.addWord(vw);
if(++count % 5000 == 0)
{
ULOGGER_DEBUG("Loaded %d words...", count);
//ULOGGER_DEBUG("Loaded %d words...", count);
}
rc = sqlite3_step(ppStmt); // next result...
}
@@ -3758,9 +3619,50 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c
// Get Last word id
getLastWordId(id);
dictionary->setLastWordId(id);
dictionary.setLastWordId(id);
ULOGGER_DEBUG("Time=%fs", timer.ticks());
if(uStrNumCmp(_version, "0.23.0") >= 0) {
// load dictionary index
std::stringstream query3;
query3 << "SELECT dictionary_index "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
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());
// 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(), query3.str().c_str()).c_str());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_poses
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
UDEBUG("A flann index was saved in the database (size=%ld).", dataSize);
dictionary.deserializeIndex((const unsigned char*)data, dataSize);
}
else {
UDEBUG("No flann index was saved in the database.");
}
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("Loaded %d words... time=%fs", count, timer.ticks());
}
}
@@ -3860,6 +3762,262 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
}
}
void DBDriverSqlite3::loadWordIdsQuery(std::list<Signature *> & signatures) const
{
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT word_id "
"FROM Feature "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query << " ORDER BY word_id"; // Needed for fast insertion below
query << ";";
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*>::const_iterator iter=signatures.begin(); iter!=signatures.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;
std::multimap<int, int> visualWords;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
visualWordId = sqlite3_column_int(ppStmt, index++);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, -1));
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, std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
}
//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());
}
}
void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
{
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "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)
{
query << "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)
{
query << "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
{
query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query << " ORDER BY word_id"; // Needed for fast insertion below
query << ";";
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());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.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, int> visualWords;
std::vector<cv::KeyPoint> visualWordsKpts;
std::vector<cv::Point3f> visualWords3;
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++);
}
visualWordsKpts.push_back(kpt);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
visualWords3.push_back(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.push_back(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
{
if(allWords3NaN)
{
visualWords3.clear();
}
(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, 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.rows, (*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());
}
}
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::multimap<int, Link> & links,
@@ -4174,7 +4332,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
//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());
//UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
}
// Finalize (delete) the statement
@@ -5117,8 +5275,8 @@ std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * la
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());
poses.insert(poses.end(), std::make_pair(serializedIds.at<int>(i), t));
//UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
}
}
@@ -5591,6 +5749,47 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
return cloud;
}
void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & data) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.23.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 dictionary_index=? 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;
if(data.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++, data.data(), data.size(), 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::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
@@ -6598,7 +6797,7 @@ void DBDriverSqlite3::stepLink(
{
UFATAL("");
}
UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
//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)

2
corelib/src/DBReader.cpp
View File

@@ -260,7 +260,7 @@ bool DBReader::init(
else
{
Signature * s = _dbDriver->loadSignature(*_ids.begin());
_dbDriver->loadNodeData(s);
_dbDriver->loadNodeData(*s);
if( s->sensorData().imageCompressed().empty() &&
s->getWords().empty() &&
!s->sensorData().laserScanCompressed().empty())

449
corelib/src/FlannIndex.cpp
View File

@@ -27,8 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/FlannIndex.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Version.h>
#ifdef WIN32
#include <rtabmap/core/Parameters.h>
#endif
#include "rtflann/flann.hpp"
#include <boost/crc.hpp>
namespace rtabmap {
@@ -37,7 +45,6 @@ FlannIndex::FlannIndex():
nextIndex_(0),
featuresType_(0),
featuresDim_(0),
isLSH_(false),
useDistanceL1_(false),
rebalancingFactor_(2.0f)
{
@@ -49,9 +56,9 @@ FlannIndex::~FlannIndex()
void FlannIndex::release()
{
UDEBUG("");
if(index_)
{
UDEBUG("Clearing flann index...");
if(featuresType_ == CV_8UC1)
{
delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
@@ -72,12 +79,139 @@ void FlannIndex::release()
}
}
index_ = 0;
UDEBUG("Clearing flann index... done!");
}
nextIndex_ = 0;
isLSH_ = false;
addedDescriptors_.clear();
removedIndexes_.clear();
UDEBUG("");
}
#define FLANN_INDEX_HEADER_SIZE 12
std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) const {
if(index_ && !addedDescriptors_.empty())
{
#ifdef WIN32
UERROR("FLANN index serialization is not yet implemented on Windows. Parameter \"%s\" cannot be used.", Parameters::kKpFlannIndexSaved().c_str());
#else
UTimer timer;
const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
long bytes_written = 0;
if (indexDataPtr) {
if(featuresType_ == CV_8UC1)
{
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->save(indexDataPtr);
}
else
{
if(useDistanceL1_)
{
((rtflann::Index<rtflann::L1<float> >*)index_)->save(indexDataPtr);;
}
else if(featuresDim_ <= 3)
{
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->save(indexDataPtr);;
}
else
{
((rtflann::Index<rtflann::L2<float> >*)index_)->save(indexDataPtr);;
}
}
bytes_written = ftell(indexDataPtr);
fclose(indexDataPtr);
}
if(bytes_written < long(indexData.size()-headerSizeBytes))
{
//Expected data size and type
int dataRows = 0;
int dataCols = 0;
int dataType = -1;
cv::Mat dataset;
std::set<int> removedDescriptors;
if(computeChecksum){
removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end());
}
for(const auto & iter: addedDescriptors_)
{
UASSERT(!iter.second.empty());
dataRows += iter.second.rows;
if(dataCols <= 0) {
dataCols = iter.second.cols;
}
else {
UASSERT(dataCols == iter.second.cols);
}
if(dataType < 0) {
dataType = iter.second.type();
}
else {
UASSERT(dataType == iter.second.type());
}
if(computeChecksum){
if(removedDescriptors.find(iter.first) == removedDescriptors.end()) {
if(dataset.empty()) {
dataset = iter.second.clone();
}
else {
dataset.push_back(iter.second);
}
}
else {
dataRows -= iter.second.rows;
}
}
}
if(!computeChecksum) {
for(const auto & index: removedIndexes_)
{
dataRows -= addedDescriptors_.at(index).rows;
}
}
unsigned int crcValue = 0;
if(computeChecksum) {
boost::crc_32_type result;
result.process_bytes(dataset.data, dataset.total()*dataset.elemSize());
crcValue = result.checksum();
}
indexData.resize(bytes_written+headerSizeBytes);
indexData.shrink_to_fit();
int rebalancingFactorAsInt;
memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_));
int crcValueAsInt;
memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
int header[FLANN_INDEX_HEADER_SIZE] = {
RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
algorithm_, // 3,
featuresDim_, // 4,
useDistanceL1_?1:0, // 5,
rebalancingFactorAsInt, // 6,
dataRows, // 7,
dataCols, // 8,
dataType, // 9,
crcValueAsInt, // 10
(int)bytes_written}; // 11
UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d",
header[0],header[1],header[2],
header[3],
header[4],
header[5],
rebalancingFactor_,
header[7], header[8], header[9], crcValueAsInt,
header[11]);
memcpy(indexData.data(), header, headerSizeBytes);
return indexData;
}
else {
UERROR("Target buffer too small to serialize index, aborting.");
}
UDEBUG("Flann serialization: %fs", timer.ticks());
#endif
}
return std::vector<unsigned char>();
}
size_t FlannIndex::indexedFeatures() const
@@ -139,12 +273,13 @@ size_t FlannIndex::memoryUsed() const
return memoryUsage;
}
void FlannIndex::buildLinearIndex(
void FlannIndex::buildIndex(
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
UDEBUG("algorithm=%d", (int)algorithm);
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -152,8 +287,29 @@ void FlannIndex::buildLinearIndex(
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
algorithm_ = algorithm;
rtflann::LinearIndexParams params;
rtflann::IndexParams params;
switch (algorithm)
{
case FLANN_INDEX_LINEAR:
params = rtflann::LinearIndexParams();
break;
case FLANN_INDEX_KDTREE:
params = rtflann::KDTreeIndexParams(4);
break;
case FLANN_INDEX_KDTREE_SINGLE:
params = rtflann::KDTreeSingleIndexParams(10, true);
break;
case FLANN_INDEX_LSH:
UASSERT(features.type() == CV_8UC1);
params = rtflann::LshIndexParams(12, 20, 2);
break;
default:
UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
break;
}
if(featuresType_ == CV_8UC1)
{
@@ -199,13 +355,140 @@ void FlannIndex::buildLinearIndex(
UDEBUG("");
}
void FlannIndex::buildKDTreeIndex(
const cv::Mat & features,
int trees,
bool useDistanceL1,
float rebalancingFactor)
bool FlannIndex::loadIndex(
const std::vector<unsigned char> & indexData,
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor,
std::string * error)
{
UDEBUG("");
return loadIndex(
indexData.data(),
indexData.size(),
algorithm,
features,
useDistanceL1,
rebalancingFactor),
error;
}
bool FlannIndex::loadIndex(
const unsigned char * indexData,
size_t indexDataSize,
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor,
std::string * error)
{
UASSERT(indexData!=NULL);
if(indexDataSize == 0) {
UWARN("Trying to load empty index....");
return false;
}
#ifdef WIN32
UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
return false;
#else
// Check if the features match the expected data from the index
size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
if(indexDataSize < headerSizeBytes) {
if(error) {
*error = uFormat("Wrong header size detected (%ld vs expected %ld).", indexDataSize, headerSizeBytes);
}
return false;
}
const int * header = (const int *)indexData;
int savedAlgorithm = header[3];
int savedDim = header[4];
bool savedDistanceL1 = header[5]==1;
float savedRebalancingFactor;
memcpy(&savedRebalancingFactor, &header[6], sizeof(header[6]));
int savedRows = header[7];
int savedCols = header[8];
int savedType = header[9];
unsigned int savedCrc;
memcpy(&savedCrc, &header[10], sizeof(header[10]));
int savedIndexSize = header[11];
UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d",
header[0],header[1],header[2],
header[3],
header[4],
header[5],
savedRebalancingFactor,
header[7], header[8], header[9], savedCrc,
header[11]);
if(savedAlgorithm != algorithm) {
if(error) {
*error = uFormat("Serialized flann algorithm (%d) doesn't match the expected one (%d).", savedAlgorithm, algorithm);
}
return false;
}
if(savedDim != features.cols) {
if(error) {
*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedDim, features.cols);
}
return false;
}
if(savedDistanceL1 != useDistanceL1) {
if(error) {
*error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false");
}
return false;
}
if(savedRebalancingFactor != rebalancingFactor) {
if(error) {
*error = uFormat("Serialized \"rebalancing factor\" (%f) doesn't match the expected one (%f).", savedRebalancingFactor, rebalancingFactor);
}
return false;
}
if(savedRows != features.rows) {
if(error) {
*error = uFormat("Serialized feature count (%d) doesn't match the expected one (%d).", savedRows, features.rows);
}
return false;
}
if(savedCols != features.cols) {
if(error) {
*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedCols, features.cols);
}
return false;
}
if(savedType != features.type()) {
if(error) {
*error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type());
}
return false;
}
if(savedCrc != 0) {
// Compute checksum and compare
boost::crc_32_type result;
result.process_bytes(features.data, features.total()*features.elemSize());
if(savedCrc != result.checksum()) {
if(error) {
*error = uFormat("Serialized feature crc (%X) doesn't match the expected one (%X).", savedCrc, result.checksum());
}
return false;
}
}
if(savedIndexSize != int(indexDataSize - headerSizeBytes)) {
if(error) {
*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes);
}
return false;
}
if(savedIndexSize == 0) {
if(error) {
*error = "Serialized flann index is empty.";
}
return false;
}
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -213,14 +496,39 @@ void FlannIndex::buildKDTreeIndex(
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
algorithm_ = algorithm;
rtflann::KDTreeIndexParams params(trees);
UDEBUG("algorithm=%d", (int)algorithm);
rtflann::IndexParams params;
switch (algorithm)
{
case FLANN_INDEX_LINEAR:
params = rtflann::LinearIndexParams();
break;
case FLANN_INDEX_KDTREE:
params = rtflann::KDTreeIndexParams(4);
break;
case FLANN_INDEX_KDTREE_SINGLE:
params = rtflann::KDTreeSingleIndexParams(10, true);
break;
case FLANN_INDEX_LSH:
UASSERT(features.type() == CV_8UC1);
params = rtflann::LshIndexParams(12, 20, 2);
break;
default:
UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
break;
}
FILE* indexDataPtr = fmemopen((void*)(indexData+headerSizeBytes), indexDataSize - headerSizeBytes, "r");
if(featuresType_ == CV_8UC1)
{
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->load_saved_index(indexDataPtr);
}
else
{
@@ -228,22 +536,24 @@ void FlannIndex::buildKDTreeIndex(
if(useDistanceL1_)
{
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L1<float> >*)index_)->load_saved_index(indexDataPtr);
}
else if(featuresDim_ <=3)
{
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->load_saved_index(indexDataPtr);
}
else
{
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L2<float> >*)index_)->load_saved_index(indexDataPtr);
}
}
fclose(indexDataPtr);
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
@@ -257,107 +567,8 @@ void FlannIndex::buildKDTreeIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
void FlannIndex::buildKDTreeSingleIndex(
const cv::Mat & features,
int leafMaxSize,
bool reorder,
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
if(featuresType_ == CV_8UC1)
{
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
}
else
{
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
if(useDistanceL1_)
{
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
}
else if(featuresDim_ <=3)
{
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
}
else
{
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
}
}
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
void FlannIndex::buildLSHIndex(
const cv::Mat & features,
unsigned int table_number,
unsigned int key_size,
unsigned int multi_probe_level,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_8UC1);
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = true;
rebalancingFactor_ = rebalancingFactor;
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
return true;
#endif
}
bool FlannIndex::isBuilt()

17
corelib/src/GlobalMap.cpp
View File

@@ -99,15 +99,12 @@ unsigned long GlobalMap::getMemoryUsed() const
return memoryUsage;
}
bool GlobalMap::update(const std::map<int, Transform> & poses)
bool GlobalMap::fullUpdateNeeded(const std::map<int, Transform> & poses) const
{
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
float updateErrorSqrd = updateError_*updateError_;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
if(jter != poses.end())
@@ -125,7 +122,15 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
}
}
if(graphOptimized || graphChanged)
return graphOptimized || graphChanged;
}
bool GlobalMap::update(const std::map<int, Transform> & poses)
{
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
if(fullUpdateNeeded(poses))
{
// clear all but keep cache
clear();

4
corelib/src/LocalGrid.cpp
View File

@@ -64,8 +64,8 @@ void LocalGridCache::add(int nodeId,
void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
{
UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels());
//UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
// nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels());
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);

178
corelib/src/Memory.cpp
View File

@@ -76,6 +76,7 @@ Memory::Memory(const ParametersMap & parameters) :
_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
_binDataKept(Parameters::defaultMemBinDataKept()),
_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
_loadVisualLocalFeaturesOnInit(Parameters::defaultMemLoadVisualLocalFeaturesOnInit()),
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
@@ -83,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
_flannIndexSaved(Parameters::defaultKpFlannIndexSaved()),
_reduceGraph(Parameters::defaultMemReduceGraph()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -245,13 +247,13 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
std::set<int> ids;
_dbDriver->getAllNodeIds(ids, true);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures, 0, !_loadVisualLocalFeaturesOnInit);
}
else
{
// load previous session working memory
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
_dbDriver->loadLastNodes(dbSignatures);
_dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit);
}
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
{
@@ -417,20 +419,22 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
}
else
{
_dbDriver->load(_vwd, false);
_dbDriver->load(*_vwd, false);
}
}
else
{
UDEBUG("load words");
// load the last dictionary
_dbDriver->load(_vwd, _vwd->isIncremental());
_dbDriver->load(*_vwd, _vwd->isIncremental());
}
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
_vwd->update();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
UDEBUG("Adding word references...");
UTimer timer;
// Enable loaded signatures
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -441,7 +445,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
const std::multimap<int, int> & words = s->getWords();
if(words.size())
{
UDEBUG("node=%d, word references=%d", s->id(), words.size());
//UDEBUG("node=%d, word references=%d", s->id(), words.size());
for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
{
if(iter->first > 0)
@@ -458,7 +462,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
}
UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
UDEBUG("Total word references added = %d (in %f s)", _vwd->getTotalActiveReferences(), timer.ticks());
if(_lastSignature == 0)
{
@@ -482,6 +486,37 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
UDEBUG("map ids start with %d", _idMapCount);
}
void Memory::saveFlannIndex(bool postInitClosingEvents)
{
if(!_dbDriver) {
return;
}
if(uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.0") >= 0) {
if(_flannIndexSaved && !_incrementalMemory) {
if(_vwd->isModified()) {
UINFO("Saving flann index to database... (%s=true)", Parameters::kKpFlannIndexSaved().c_str());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database..."));
_dbDriver->saveFlannIndex(_vwd->serializeIndex());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database, done!"));
}
else
{
UDEBUG("The dictionary didn't change since loaded, do not need to save again to database.");
}
}
else {
// clear if exists
_dbDriver->saveFlannIndex(std::vector<unsigned char>());
}
}
else if(_flannIndexSaved)
{
UWARN("Parameter %s is enabled, but database version is too old (%s < 0.23). Flann index cannot be saved.",
Parameters::kKpFlannIndexSaved().c_str(),
_dbDriver->getDatabaseVersion().c_str());
}
}
void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
@@ -493,6 +528,8 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
}
UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0);
if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
{
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
@@ -500,6 +537,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
UINFO("No changes added to database.");
if(_dbDriver)
{
saveFlannIndex(postInitClosingEvents);
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection(false, ouputDatabasePath);
delete _dbDriver;
@@ -514,11 +552,15 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
{
UINFO("Saving memory...");
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
if(!_memoryChanged && _linksChanged && _dbDriver)
if(!_memoryChanged && _dbDriver)
{
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
saveFlannIndex(postInitClosingEvents);
if(_linksChanged) {
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
}
}
this->clear();
if(_dbDriver)
@@ -565,6 +607,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(params, Parameters::kMemLoadVisualLocalFeaturesOnInit(), _loadVisualLocalFeaturesOnInit);
Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
@@ -618,6 +661,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved);
if(_markerAngVariance>=9999)
{
@@ -654,10 +698,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
// Keypoint stuff
if(_vwd)
{
_vwd->parseParameters(params);
}
_vwd->parseParameters(params);
Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
@@ -856,7 +897,7 @@ void Memory::preUpdate()
{
this->cleanUnusedWords();
}
if(_vwd && !_parallelized)
if(!_parallelized)
{
//When parallelized, it is done in CreateSignature
_vwd->update();
@@ -1114,10 +1155,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
}
++_signaturesAdded;
if(_vwd)
{
UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
}
UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
if(signature->getWords().size())
{
signature->setEnabled(true);
@@ -1836,13 +1874,24 @@ void Memory::clear()
UDEBUG("");
//Get the tree root (parents)
std::map<int, Signature*> mem = _signatures;
for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
{
if(i->second)
if(!_dbDriver) {
// We are not saving to database anyway, just delete now.
for(std::map<int, Signature *>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
UDEBUG("deleting from the working and the short-term memory: %d", i->first);
this->moveToTrash(i->second);
delete iter->second;
}
_workingMem.clear();
_signatures.clear();
}
else {
std::map<int, Signature*> mem = _signatures;
for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
{
if(i->second)
{
//UDEBUG("deleting from the working and the short-term memory: %d", i->first);
this->moveToTrash(i->second);
}
}
}
@@ -1866,6 +1915,7 @@ void Memory::clear()
UDEBUG("");
_lastSignature = 0;
_lastGlobalLoopClosureId = 0;
_signaturesAdded = 0;
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
@@ -1886,14 +1936,7 @@ void Memory::clear()
cleanUnusedWords();
_dbDriver->emptyTrashes();
}
else
{
cleanUnusedWords();
}
if(_vwd)
{
_vwd->clear();
}
_vwd->clear(_dbDriver!=NULL);
UDEBUG("");
}
@@ -2428,7 +2471,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
*/
void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
{
UDEBUG("id=%d", s?s->id():0);
//UDEBUG("id=%d", s?s->id():0);
if(s)
{
// Cleanup landmark indexes
@@ -2921,6 +2964,37 @@ Transform Memory::computeTransform(
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
// Load word descriptors and keypoints on-demand if necessary
if( !_reextractLoopClosureFeatures &&
(_registrationPipeline->isImageRequired() || guess.isNull()) &&
!fromS.getWords().empty() && fromS.getWordsKpts().empty() &&
_dbDriver)
{
// We assume "toS" has already features in RAM, so just lookup "fromS"
UDEBUG("Loading local visual features for signature %d", fromS.id());
std::multimap<int, int> words;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
cv::Mat descriptors;
UTimer timer;
_dbDriver->getLocalFeatures(fromS.id(), words, keypoints, points, descriptors);
if(!words.empty() && !keypoints.empty()) {
UASSERT(words.size() == fromS.getWords().size());
std::map<int, int> wordsChanged = fromS.getWordsChanged();
bool wasEnabled = fromS.isEnabled();
fromS.setWords(words, keypoints, points, descriptors);
for(const auto & iter: wordsChanged) {
fromS.changeWordsRef(iter.first, iter.second);
}
fromS.setEnabled(wasEnabled);
UDEBUG("Loaded %ld local visual features for signature %d! (in %f s)", words.size(), fromS.id(), timer.ticks());
}
else
{
UDEBUG("Failed to load local visual features for signature %d.", fromS.id());
}
}
// compute transform fromId -> toId
std::vector<int> inliersV;
@@ -2980,8 +3054,10 @@ Transform Memory::computeTransform(
!_invertedReg &&
!tmpTo.getWordsDescriptors().empty() &&
!tmpTo.getWords().empty() &&
!tmpTo.getWordsKpts().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
!tmpFrom.getWords().empty() &&
!tmpFrom.getWordsKpts().empty() &&
!tmpFrom.getWords3().empty() &&
fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
{
@@ -3015,8 +3091,12 @@ Transform Memory::computeTransform(
if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
{
const Signature * s = this->getSignature(id);
if(s && !s->getWords3().empty())
if(s)
{
if(s->getWordsKpts().empty() && s->getWords3().empty() && s->getWordsDescriptors().empty()) {
UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id());
continue;
}
const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
{
@@ -3115,6 +3195,11 @@ Transform Memory::computeTransform(
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
if(s->getWordsKpts().empty())
{
UDEBUG("Signature %d doesn't have features set. Keypoints won't be added in local bundle adjustment.", s->id());
continue;
}
const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
@@ -3590,7 +3675,7 @@ void Memory::removeAllVirtualLinks()
void Memory::removeVirtualLinks(int signatureId)
{
UDEBUG("");
//UDEBUG("");
Signature * s = this->_getSignature(signatureId);
if(s)
{
@@ -3629,10 +3714,7 @@ void Memory::dumpMemory(std::string directory) const
void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
{
if(_vwd)
{
_vwd->exportDictionary(fileNameRef, fileNameDesc);
}
_vwd->exportDictionary(fileNameRef, fileNameDesc);
}
void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
@@ -3753,10 +3835,7 @@ unsigned long Memory::getMemoryUsed() const
{
memoryUsage += iter->second->getMemoryUsed(true);
}
if(_vwd)
{
memoryUsage += _vwd->getMemoryUsed();
}
memoryUsage += _vwd->getMemoryUsed();
memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map<int, double>::iterator)) + sizeof(std::map<int, double>);
memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
@@ -4221,6 +4300,11 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
}
}
}
if(!words.empty() && wordsKpts.empty() && _dbDriver)
{
std::multimap<int, int> tmpWords;
_dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors);
}
}
void Memory::getNodeCalibration(int nodeId,
@@ -6330,7 +6414,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
void Memory::disableWordsRef(int signatureId)
{
UDEBUG("id=%d", signatureId);
//UDEBUG("id=%d", signatureId);
Signature * ss = this->_getSignature(signatureId);
if(ss && ss->isEnabled())
@@ -6346,7 +6430,7 @@ void Memory::disableWordsRef(int signatureId)
count -= _vwd->getTotalActiveReferences();
ss->setEnabled(false);
UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
//UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
}
}
@@ -6409,7 +6493,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
// the words were deleted, so try to math it with an active word
// the words were deleted, so try to match it with an active word
std::list<VisualWord *> vws;
if(oldWordIds.size() && _dbDriver)
{

4
corelib/src/Optimizer.cpp
View File

@@ -608,8 +608,8 @@ void Optimizer::computeBACorrespondences(
}
}
if(sFrom.getWords().size() &&
sTo.getWords().size() &&
if(sFrom.getWordsKpts().size() &&
sTo.getWordsKpts().size() &&
sFrom.getWords3().size())
{
if(!rematchFeatures)

10
corelib/src/RegistrationVis.cpp
View File

@@ -375,6 +375,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
UDEBUG("");
// just some checks to make sure that input data are ok
UASSERT(fromSignature.getWords().empty() ||
fromSignature.getWordsKpts().empty() ||
(fromSignature.getWords().size() == fromSignature.getWordsKpts().size()));
UASSERT(fromSignature.getWords().empty() ||
fromSignature.getWords3().empty() ||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
@@ -382,8 +385,11 @@ Transform RegistrationVis::computeTransformationImpl(
(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
fromSignature.sensorData().descriptors().empty() ||
fromSignature.getWordsDescriptors().empty() == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() && toSignature.getWords3().empty())||
UASSERT(toSignature.getWords().empty() ||
toSignature.getWordsKpts().empty() ||
(toSignature.getWords().size() == toSignature.getWordsKpts().size()));
UASSERT(toSignature.getWords().empty() ||
toSignature.getWords3().empty() ||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||

6
corelib/src/SensorData.cpp
View File

@@ -548,7 +548,7 @@ void SensorData::setOccupancyGrid(
float cellSize,
const cv::Point3f & viewPoint)
{
UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
//UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
(!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
@@ -649,7 +649,7 @@ void SensorData::uncompressData(
cv::Mat * emptyCellsRaw,
cv::Mat * depthConfidenceRaw)
{
UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)",
/*UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)",
this->id(),
imageRaw?1:0,
depthRaw?1:0,
@@ -658,7 +658,7 @@ void SensorData::uncompressData(
groundCellsRaw?1:0,
obstacleCellsRaw?1:0,
emptyCellsRaw?1:0,
depthConfidenceRaw?1:0);
depthConfidenceRaw?1:0);*/
if(imageRaw == 0 &&
depthRaw == 0 &&
laserScanRaw == 0 &&

6
corelib/src/Signature.cpp
View File

@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
}
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
//UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
@@ -318,7 +318,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
//UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
_invalidWordsCount = 0;
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
@@ -328,7 +328,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
++_invalidWordsCount;
}
// make sure indexes are all valid!
UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
UASSERT_MSG(iter->second<0 || iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
}
_enabled = false;

221
corelib/src/VWDictionary.cpp
View File

@@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <fstream>
#include <string>
#define KDTREE_SIZE 4
#define KNN_CHECKS 32
namespace rtabmap
@@ -69,9 +68,11 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_nndrRatio(Parameters::defaultKpNndrRatio()),
_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
_serializeWithChecksum(Parameters::defaultKpSerializeWithChecksum()),
_lastWordId(0),
useDistanceL1_(false),
_flannIndex(new FlannIndex()),
_modified(true),
_strategy(kNNBruteForce)
{
this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -89,6 +90,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
bool byteToFloat = _byteToFloat;
@@ -160,7 +162,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
DBDriver * driver = DBDriver::create();
if(driver->openConnection(dictionaryPath, false))
{
driver->load(this, false);
driver->load(*this, false);
for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
{
iter->second->setSaved(true);
@@ -289,6 +291,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
_newDictionaryPath = dictionaryPath;
}
bool VWDictionary::isModified() const
{
return _modified;
}
bool VWDictionary::setNNStrategy(NNStrategy strategy)
{
#if CV_MAJOR_VERSION < 3
@@ -484,7 +491,13 @@ void VWDictionary::update()
if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
{
if(_incrementalFlann &&
_modified = true;
bool firstUpdate = _removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size();
UDEBUG("firstUpdate=%s (_removedIndexedWords=%ld, _visualWords=%ld, _notIndexedWords=%ld)",
firstUpdate?"true":"false", _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
if(!firstUpdate &&
_incrementalFlann &&
_strategy < kNNBruteForce &&
_visualWords.size())
{
@@ -501,7 +514,9 @@ void VWDictionary::update()
if(_notIndexedWords.size())
{
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
UTimer timer;
timer.start();
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -528,24 +543,13 @@ void VWDictionary::update()
int index = 0;
if(!_flannIndex->isBuilt())
{
UDEBUG("Building FLANN index...");
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
break;
default:
UFATAL("Not supposed to be here!");
break;
}
UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)",
nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor);
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
descriptor, useDistanceL1_, _rebalancingFactor);
UDEBUG("Building FLANN index... done!");
}
else
@@ -561,7 +565,7 @@ void VWDictionary::update()
inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
UASSERT(inserted.second);
}
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks());
}
}
else if(_strategy >= kNNBruteForce &&
@@ -657,23 +661,13 @@ void VWDictionary::update()
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
default:
break;
}
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
_dataTree,
useDistanceL1_,
_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
}
}
@@ -689,6 +683,146 @@ void VWDictionary::update()
UDEBUG("");
}
std::vector<unsigned char> VWDictionary::serializeIndex() const
{
if(_strategy >= kNNBruteForce) {
UINFO("Not flann strategy, ignoring serialization...");
return std::vector<unsigned char>();
}
if(!_flannIndex->isBuilt() || !_removedIndexedWords.empty() || !_notIndexedWords.empty() || _visualWords.empty()) {
UWARN("Flann index is not buit, or there are words not indexed, cannot do serialization.");
return std::vector<unsigned char>();
}
return _flannIndex->serializeIndex(_serializeWithChecksum);
}
void VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
{
deserializeIndex(data.data(), data.size());
}
void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
{
if(data== NULL || size == 0)
{
UWARN("Trying to deserialize empty data, aborting.");
return;
}
UDEBUG("Loading flann index... (data size=%ld bytes)", size);
if(_strategy >= kNNBruteForce) {
//ignore
return;
}
if(_flannIndex->isBuilt()) {
UERROR("Flann index is already built, cannot deserialize data!");
return;
}
if(_visualWords.empty()) {
UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()");
return;
}
if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) {
UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)",
_removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
return;
}
std::map<int, int> mapIndexId;
std::map<int, int> mapIdIndex;
cv::Mat dataTree;
UTimer timer;
timer.start();
int dim = _visualWords.begin()->second->getDescriptor().cols;
int type;
if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
{
useDistanceL1_ = true;
if(_strategy == kNNFlannKdTree)
{
type = CV_32F;
if(!_byteToFloat)
{
dim *= 8;
}
}
else
{
type = _visualWords.begin()->second->getDescriptor().type();
}
}
else
{
type = _visualWords.begin()->second->getDescriptor().type();
}
UASSERT(type == CV_32F || type == CV_8U);
UASSERT(dim > 0);
// Create the data matrix
dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
cv::Mat descriptor;
if(iter->second->getDescriptor().type() == CV_8U)
{
if(_strategy == kNNFlannKdTree)
{
descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
}
else
{
descriptor = iter->second->getDescriptor();
}
}
else
{
descriptor = iter->second->getDescriptor();
}
UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
descriptor.copyTo(dataTree.row(i));
mapIndexId.insert(mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
mapIdIndex.insert(mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
}
ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
std::string errorMsg;
if(_flannIndex->loadIndex(
data,
size,
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE,
dataTree,
useDistanceL1_,
_incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1,
&errorMsg))
{
_mapIndexId = mapIndexId;
_mapIdIndex = mapIdIndex;
_dataTree = dataTree;
_notIndexedWords.clear();
_modified = false;
}
else {
UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str());
_flannIndex->release(); // reset to initial state
}
ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks());
}
void VWDictionary::clear(bool printWarningsIfNotEmpty)
{
ULOGGER_DEBUG("");
@@ -718,6 +852,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
_unusedWords.clear();
_flannIndex->release();
useDistanceL1_ = false;
_modified = true;
}
int VWDictionary::getNextId()
@@ -1394,15 +1529,15 @@ void VWDictionary::addWord(VisualWord * vw)
{
if(vw)
{
_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(vw->id());
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
if(vw->getReferences().size())
{
_totalActiveReferences += uSum(uValues(vw->getReferences()));
}
else
{
_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
}
if(_lastWordId < vw->id())
{

2
corelib/src/VisualWord.cpp
View File

@@ -57,7 +57,7 @@ void VisualWord::addRef(int signatureId)
}
else
{
_references.insert(std::pair<int, int>(signatureId, 1));
_references.insert(_references.end(), std::pair<int, int>(signatureId, 1));
}
++_totalReferences;
}

2
corelib/src/odometry/OdometryF2F.cpp
View File

@@ -176,6 +176,8 @@ Transform OdometryF2F::computeTransform(
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<int, int> > > pairs;
UASSERT(tmpRefFrame.getWords().size() == tmpRefFrame.getWordsKpts().size());
UASSERT(newFrame.getWords().size() == newFrame.getWordsKpts().size());
EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());

1
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -793,6 +793,7 @@ Transform OdometryF2M::computeTransform(
if(!lastFrameModels.empty())
{
UASSERT(lastFrame_->getWordsKpts().size() == lastFrame_->getWords().size());
for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];

4
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -131,7 +131,9 @@ CREATE TABLE Admin (
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
opt_map_resolution FLOAT,
opt_map_resolution FLOAT,
dictionary_index BLOB, -- serialized dictionary index
time_enter DATE
);

183
corelib/src/resources/backward_compatibility/DatabaseSchema_0_22_0.sql Normal file
View File

@@ -0,0 +1,183 @@
-- *******************************************************************
-- DatabaseSchema: Script for creating the database
-- Usage:
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
-- *******************************************************************
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Node;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
map_id INTEGER NOT NULL,
weight INTEGER,
stamp FLOAT,
pose BLOB, -- 3x4 float
ground_truth_pose BLOB, -- 3x4 float
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
env_sensors BLOB, -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Data (
id INTEGER NOT NULL,
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
depth_confidence BLOB, -- compressed data (low=0 high=100)
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_info BLOB, -- scan_max_pts, scan_max_range, scan_format, local_transform
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)
empty_cells BLOB, -- compressed data (occupancy grid)
cell_size FLOAT,
view_point_x FLOAT,
view_point_y FLOAT,
view_point_z FLOAT,
user_data BLOB, -- compressed data (User data)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
transform BLOB, -- 3x4 float
user_data BLOB, -- compressed data (User data)
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
--
CREATE TABLE Word (
id INTEGER NOT NULL,
descriptor_size INTEGER NOT NULL,
descriptor BLOB NOT NULL,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Feature (
node_id INTEGER NOT NULL,
word_id INTEGER NOT NULL,
pos_x FLOAT NOT NULL,
pos_y FLOAT NOT NULL,
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
response FLOAT NOT NULL,
octave INTEGER NOT NULL,
depth_x FLOAT,
depth_y FLOAT,
depth_z FLOAT,
descriptor_size INTEGER,
descriptor BLOB,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE GlobalDescriptor (
node_id INTEGER NOT NULL,
type INTEGER NOT NULL,
info BLOB,
data BLOB NOT NULL,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
--
CREATE TABLE Info (
STM_size INTEGER,
last_sign_added INTEGER,
process_mem_used INTEGER,
database_mem_used INTEGER,
dictionary_size INTEGER,
parameters TEXT,
time_enter DATE
);
CREATE TABLE Statistics (
id INTEGER NOT NULL,
stamp FLOAT,
data BLOB, -- compressed string
wm_state BLOB, -- compressed data
FOREIGN KEY (id) REFERENCES Node(id)
);
CREATE TABLE Admin (
version TEXT,
preview_image BLOB, -- compressed image
opt_cloud BLOB, -- compressed data
opt_ids BLOB, -- Node ids used to generate the optimized cloud/mesh
opt_poses BLOB, -- compressed N*3x4 float
opt_last_localization BLOB, -- 3x4 float
opt_polygons_size INTEGER, -- e.g., 3
opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
opt_tex_materials BLOB, -- compressed image
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
opt_map_resolution FLOAT,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
END;
-- Creating a trigger for time_enter
CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
BEGIN
UPDATE Word SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
BEGIN
UPDATE Info SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- *******************************************************************
-- INDEXES
-- *******************************************************************
CREATE UNIQUE INDEX IDX_Node_id on Node (id);
CREATE INDEX IDX_Feature_node_id on Feature (node_id);
CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
-- *******************************************************************
-- VERSION
-- *******************************************************************
INSERT INTO Admin(version) VALUES('0.22.0');

33
corelib/src/rtflann/flann.hpp
View File

@@ -103,7 +103,6 @@ public:
{
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
loaded_ = false;
if (index_type == FLANN_INDEX_SAVED) {
nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
loaded_ = true;
@@ -180,10 +179,20 @@ public:
if (fout == NULL) {
throw FLANNException("Cannot open file");
}
nnIndex_->saveIndex(fout);
save(fout);
fclose(fout);
}
/**
* Save index to file stream.
* Caller has to open file stream with "wb" and close it afterwards.
* @param filename
*/
void save(FILE * stream)
{
nnIndex_->saveIndex(stream);
}
/**
* \returns number of features in this index.
*/
@@ -377,6 +386,26 @@ public:
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
void load_saved_index(FILE* fin)
{
if(loaded_) {
throw FLANNException("Index already loaded!");
}
if(nnIndex_->sizeAtBuild() != 0) {
throw FLANNException("Index must not be already built to load data.");
}
if (fin == NULL) {
throw FLANNException("File pointer must be valid!");
}
IndexHeader header = load_header(fin);
if (header.h.data_type != flann_datatype_value<ElementType>::value) {
throw FLANNException("Datatype of saved index is different than of the one to be loaded.");
}
rewind(fin);
nnIndex_->loadIndex(fin);
loaded_ = true;
}
private:
IndexType* load_saved_index(const Matrix<ElementType>& dataset, const std::string& filename, Distance distance)
{