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

Fixed calibrations not loaded for bad signatures

Browse Source
This commit is contained in:
matlabbe
2016-08-10 10:35:53 -04:00
parent 0152941ecf
commit 0ccb69bc06
2 changed files with 108 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1791,7 +1791,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0); 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()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
std::set<int> calibrationsToLoad;
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{ {
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id()); //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
@@ -1867,7 +1866,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
} }
else else
{ {
calibrationsToLoad.insert((*iter)->id());
(*iter)->setWords(visualWords); (*iter)->setWords(visualWords);
(*iter)->setWords3(visualWords3); (*iter)->setWords3(visualWords3);
(*iter)->setWordsDescriptors(descriptors); (*iter)->setWordsDescriptors(descriptors);
@@ -1893,7 +1891,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
ULOGGER_DEBUG("Time load links=%fs", timer.ticks()); ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
// load calibrations // load calibrations
if(calibrationsToLoad.size() && uStrNumCmp(_version, "0.10.0") >= 0) if(nodes.size() && uStrNumCmp(_version, "0.10.0") >= 0)
{ {
std::stringstream query3; std::stringstream query3;
query3 << "SELECT calibration " query3 << "SELECT calibration "
@@ -1905,8 +1903,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int calibrationsLoaded = 0; int calibrationsLoaded = 0;
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
if(calibrationsToLoad.find((*iter)->id())!=calibrationsToLoad.end())
{ {
// bind id // bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id()); rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
@@ -2001,12 +1997,11 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
rc = sqlite3_reset(ppStmt); rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
} }
}
// Finalize (delete) the statement // Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt); rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time load calibrations (loaded=%d/%d)=%fs", calibrationsLoaded, calibrationsToLoad.size(), timer.ticks()); ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
} }
if(ids.size() != loaded) if(ids.size() != loaded)

12
corelib/src/OptimizerG2O.cpp
View File

@@ -620,10 +620,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
else else
{ {
UERROR("Did not find node %d in cache", iter->first); UERROR("Did not find node %d in cache", iter->first);
return optimizedPoses;
} }
if(model.isValidForProjection()) UASSERT(model.isValidForProjection());
{
models.insert(std::make_pair(iter->first, model)); models.insert(std::make_pair(iter->first, model));
Transform camPose = iter->second * model.localTransform(); Transform camPose = iter->second * model.localTransform();
//iter->second = (iter->second * model.localTransform()).inverse(); //iter->second = (iter->second * model.localTransform()).inverse();
@@ -642,16 +643,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
vCam->setFixed(true); vCam->setFixed(true);
} }
vCam->setId(iter->first); vCam->setId(iter->first);
std::cout << cam << std::endl; //std::cout << cam << std::endl;
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str()); UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
++iter; ++iter;
} }
else
{
frames.erase(iter++);
}
}
UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it..."); UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)