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

Added DBDriver::setTimestampUpdateEnabled()

Browse Source 
Don't update timestamps if only links are modified
This commit is contained in:
Mathieu Labbe
2015-02-04 17:40:16 -05:00
parent 09625b766e
commit f8b625c7c7
10 changed files with 136 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -72,6 +72,7 @@ public:
void asyncSave(VisualWord * vw); //ownership transferred
void emptyTrashes(bool async = false);
double getEmptyTrashesTime() const {return _emptyTrashesTime;}
void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
public:
void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
@@ -121,8 +122,8 @@ private:
virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
// Load objects
@@ -157,6 +158,7 @@ private:
USemaphore _addSem;
double _emptyTrashesTime;
std::string _url;
bool _timestampUpdate;
};
}

9
corelib/include/rtabmap/core/Memory.h
View File

@@ -80,8 +80,8 @@ public:
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
void updateLink(int fromId, int toId, const Transform & transform, float variance);
void removeAllVirtualLinks();
std::map<int, int> getNeighborsId(int signatureId,
int margin,
@@ -90,7 +90,7 @@ public:
bool ignoreLoopIds = false,
double * dbAccessTime = 0) const;
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void rejectLoopClosure(int oldId, int newId);
void removeLink(int idA, int idB);
//getters
const std::set<int> & getWorkingMem() const {return _workingMem;}
@@ -214,7 +214,8 @@ private:
Signature * _lastSignature;
int _lastGlobalLoopClosureParentId;
int _lastGlobalLoopClosureChildId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
bool _memoryChanged; // False by default, become true only when Memory::update() is called.
bool _linksChanged; // False by default, become true when links are modified.
int _signaturesAdded;
bool _postInitClosingEvents;

7
corelib/src/DBDriver.cpp
View File

@@ -38,7 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_emptyTrashesTime(0)
_emptyTrashesTime(0),
_timestampUpdate(true)
{
this->parseParameters(parameters);
}
@@ -242,7 +243,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
if(toUpdate.size())
{
this->updateQuery(toUpdate);
this->updateQuery(toUpdate, _timestampUpdate);
}
if(toSave.size())
{
@@ -272,7 +273,7 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
if(toUpdate.size())
{
this->updateQuery(toUpdate);
this->updateQuery(toUpdate, _timestampUpdate);
}
if(toSave.size())
{

17
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1548,7 +1548,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
}
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
{
UDEBUG("nodes = %d", nodes.size());
if(_ppDb && nodes.size())
@@ -1559,7 +1559,15 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
sqlite3_stmt * ppStmt = 0;
Signature * s = 0;
std::string query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
std::string query;
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", sqlite3_errmsg(_ppDb)).c_str());
@@ -1655,10 +1663,11 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
}
}
void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words) const
void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
{
if(_ppDb && words.size())
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;

4
corelib/src/DBDriverSqlite3.h
View File

@@ -60,8 +60,8 @@ private:
virtual void saveQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::list<VisualWord *> & words) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void updateQuery(const std::list<VisualWord *> & words) const;
virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const;

130
corelib/src/Memory.cpp
View File

@@ -72,6 +72,7 @@ Memory::Memory(const ParametersMap & parameters) :
_lastGlobalLoopClosureParentId(0),
_lastGlobalLoopClosureChildId(0),
_memoryChanged(false),
_linksChanged(false),
_signaturesAdded(0),
_postInitClosingEvents(false),
@@ -131,7 +132,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
DBDriver * tmpDriver = 0;
if(!_memoryChanged)
if(!_memoryChanged && !_linksChanged)
{
if(_dbDriver)
{
@@ -139,6 +140,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
_dbDriver = 0; // HACK for the clear() below to think that there is no db
}
}
else if(!_memoryChanged && _linksChanged)
{
_dbDriver->setTimestampUpdateEnabled(false); // update links only
}
this->clear();
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
@@ -162,6 +167,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
bool success = true;
if(_dbDriver)
{
_dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
success = false;
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
if(_dbDriver->openConnection(dbUrl, dbOverwritten))
@@ -312,7 +318,7 @@ Memory::~Memory()
{
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
UDEBUG("");
if(!_memoryChanged)
if(!_memoryChanged && !_linksChanged)
{
UDEBUG("");
if(_dbDriver)
@@ -331,6 +337,11 @@ Memory::~Memory()
{
UDEBUG("");
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
if(!_memoryChanged && _linksChanged && _dbDriver)
{
// don't update the time stamps!
_dbDriver->setTimestampUpdateEnabled(false);
}
this->clear();
if(_dbDriver)
{
@@ -946,11 +957,14 @@ void Memory::clear()
_workingMem.size(), _stMem.size(), _signatures.size()).c_str());
UDEBUG("Adding statistics after run...");
_dbDriver->addStatisticsAfterRun(memSize,
_lastSignature?_lastSignature->id():0,
UProcessInfo::getMemoryUsage(),
_dbDriver->getMemoryUsed(),
(int)_vwd->getVisualWords().size());
if(_memoryChanged)
{
_dbDriver->addStatisticsAfterRun(memSize,
_lastSignature?_lastSignature->id():0,
UProcessInfo::getMemoryUsage(),
_dbDriver->getMemoryUsed(),
(int)_vwd->getVisualWords().size());
}
}
UDEBUG("");
@@ -994,6 +1008,7 @@ void Memory::clear()
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
_linksChanged = false;
if(_dbDriver)
{
@@ -1527,43 +1542,63 @@ void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
}
}
void Memory::rejectLoopClosure(int oldId, int newId)
void Memory::removeLink(int oldId, int newId)
{
Signature * oldS = this->_getSignature(oldId);
Signature * newS = this->_getSignature(newId);
//this method assumes receiving oldId < newId, if not switch them
Signature * oldS = this->_getSignature(oldId<newId?oldId:newId);
Signature * newS = this->_getSignature(oldId<newId?newId:oldId);
if(oldS && newS)
{
UDEBUG("removing loop closure from location %d", newS->id());
oldS->removeLink(newS->id());
oldS->setWeight(oldS->getWeight()+1);
UINFO("removing link between location %d and %d", oldS->id(), newS->id());
newS->removeLink(oldS->id());
newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
bool noChildrenAnymore = true;
for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
{
if(iter->second.type() > Link::kNeighbor && iter->first < newS->id())
Link::Type type = oldS->getLinks().at(newS->id()).type();
if(type == Link::kGlobalClosure && newS->getWeight() > 0)
{
noChildrenAnymore = false;
break;
// adjust the weight
oldS->setWeight(oldS->getWeight()+1);
newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
}
oldS->removeLink(newS->id());
newS->removeLink(oldS->id());
if(type!=Link::kVirtualClosure)
{
_linksChanged = true;
}
bool noChildrenAnymore = true;
for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
{
if(iter->second.type() > Link::kNeighbor && iter->first < newS->id())
{
noChildrenAnymore = false;
break;
}
}
if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureParentId)
{
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
}
}
if(noChildrenAnymore && newId == _lastGlobalLoopClosureParentId)
else
{
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
}
}
else
{
if(!newS)
{
UERROR("Signature %d is not in working memory... cannot remove loop closure links.", newS->id());
UERROR("Signature %d is not in working memory... cannot remove link.", newS->id());
}
if(!oldS)
{
UERROR("Signature %d is not in working memory... cannot remove loop closure links.", oldS->id());
UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id());
}
}
}
@@ -2118,7 +2153,7 @@ Transform Memory::computeScanMatchingTransform(
}
// Transform from new to old
bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
{
UASSERT(type > Link::kNeighbor && type != Link::kUndef);
@@ -2134,23 +2169,32 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
return true;
}
if(type != Link::kVirtualClosure)
{
_memoryChanged = true;
}
UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id());
UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance));
newS->addLink(Link(newS->id(), oldS->id(), type, transform, variance));
if(type!=Link::kVirtualClosure)
{
_linksChanged = true;
}
if(_incrementalMemory && type == Link::kGlobalClosure)
{
_lastGlobalLoopClosureParentId = newS->id();
_lastGlobalLoopClosureChildId = oldS->id();
_lastGlobalLoopClosureParentId = newS->id()>oldS->id()?newS->id():oldS->id();
_lastGlobalLoopClosureChildId = newS->id()>oldS->id()?oldS->id():newS->id();
// udpate weights only if the memory is incremental
newS->setWeight(newS->getWeight() + oldS->getWeight());
oldS->setWeight(0);
if(newS->id() > oldS->id())
{
newS->setWeight(newS->getWeight() + oldS->getWeight());
oldS->setWeight(0);
}
else
{
oldS->setWeight(oldS->getWeight() + newS->getWeight());
newS->setWeight(0);
}
}
return true;
}
@@ -2168,22 +2212,28 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
return false;
}
void Memory::updateNeighborLink(int fromId, int toId, const Transform & transform, float variance)
void Memory::updateLink(int fromId, int toId, const Transform & transform, float variance)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
if(fromS->hasLink(toId) && toS->hasLink(fromId))
{
Link::Type type = fromS->getLinks().at(toId).type();
fromS->removeLink(toId);
toS->removeLink(fromId);
fromS->addLink(Link(fromId, toId, Link::kNeighbor, transform, variance));
toS->addLink(Link(toId, fromId, Link::kNeighbor, transform.inverse(), variance));
fromS->addLink(Link(fromId, toId, type, transform, variance));
toS->addLink(Link(toId, fromId, type, transform.inverse(), variance));
if(type!=Link::kVirtualClosure)
{
_linksChanged = true;
}
}
else
{
UERROR("fromId=%d and toId=%d are not neighbors!", fromId, toId);
UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
}
}

15
corelib/src/Rtabmap.cpp
View File

@@ -865,7 +865,7 @@ bool Rtabmap::process(const SensorData & data)
oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t, variance);
_memory->updateLink(signature->id(), oldId, t, variance);
}
else
{
@@ -923,7 +923,7 @@ bool Rtabmap::process(const SensorData & data)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1370,7 +1370,7 @@ bool Rtabmap::process(const SensorData & data)
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLoopClosureLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
}
if(rejectedHypothesis)
@@ -1392,7 +1392,7 @@ bool Rtabmap::process(const SensorData & data)
!signature->hasLink(_path[_pathCurrentIndex]))
{
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex]);
_memory->addLoopClosureLink(_path[_pathCurrentIndex], signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
_memory->addLink(_path[_pathCurrentIndex], signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
}
timeAddLoopClosureLink = timer.ticks();
@@ -1441,7 +1441,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1876,7 +1876,7 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
_loopClosureHypothesis.first = 0;
if(_memory)
{
_memory->rejectLoopClosure(oldId, newId);
_memory->removeLink(oldId, newId);
}
if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
{
@@ -2031,8 +2031,7 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(fromS->getLinks().find(ids[ind[i]]) == fromS->getLinks().end() && // can't be a neighbor
(minDistance == -1 || minDistance > dist[i]))
if(minDistance == -1 || minDistance > dist[i])
{
nearestId = ids[ind[i]];
minDistance = dist[i];

9
corelib/src/util3d.cpp
View File

@@ -2308,10 +2308,11 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
pcl::PointXYZ min, max;
pcl::getMinMax3D(minMax, min, max);
xMin = min.x-cellSize;
yMin = min.y-cellSize;
float xMax = max.x+cellSize;
float yMax = max.y+cellSize;
// Added X2 to make sure that all points are inside the map (when rounded to integer)
xMin = min.x-cellSize*2.0f;
yMin = min.y-cellSize*2.0f;
float xMax = max.x+cellSize*2.0f;
float yMax = max.y+cellSize*2.0f;
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);