Added DBDriver::setTimestampUpdateEnabled()

Don't update timestamps if only links are modified

corelib/include/rtabmap/core/DBDriver.h

@@ -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<Signature *> & signatures, bool updateTimestamp) const = 0;
-	virtual void updateQuery(const std::list<VisualWord *> & words) 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;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -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);
+	bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform, 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;
 

corelib/src/DBDriver.cpp

@@ -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())
 		{

corelib/src/DBDriverSqlite3.cpp

@@ -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;

corelib/src/DBDriverSqlite3.h

@@ -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<Signature *> & signatures, bool updateTimestamp) const;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;

corelib/src/Memory.cpp

@@ -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,12 +957,15 @@ void Memory::clear()
 						_workingMem.size(), _stMem.size(), _signatures.size()).c_str());
 
 		UDEBUG("Adding statistics after run...");
+		if(_memoryChanged)
+		{
 			_dbDriver->addStatisticsAfterRun(memSize,
 					_lastSignature?_lastSignature->id():0,
 					UProcessInfo::getMemoryUsage(),
 					_dbDriver->getMemoryUsed(),
 					(int)_vwd->getVisualWords().size());
 		}
+	}
 	UDEBUG("");
 
 	//Get the tree root (parents)
@@ -994,6 +1008,7 @@ void Memory::clear()
 	_idCount = kIdStart;
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
+	_linksChanged = false;
 
 	if(_dbDriver)
 	{
@@ -1527,18 +1542,33 @@ 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);
+	//this method assumes receiving oldId < newId, if not switch them
-	Signature * newS = this->_getSignature(newId);
+	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());
+		UINFO("removing link between location %d and %d", oldS->id(), newS->id());
-		oldS->removeLink(newS->id());
-		oldS->setWeight(oldS->getWeight()+1);
 
-		newS->removeLink(oldS->id());
+		if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
+		{
+			Link::Type type = oldS->getLinks().at(newS->id()).type();
+			if(type == Link::kGlobalClosure && newS->getWeight() > 0)
+			{
+				// 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)
@@ -1549,21 +1579,26 @@ void Memory::rejectLoopClosure(int oldId, int newId)
 					break;
 				}
 			}
-		if(noChildrenAnymore && newId == _lastGlobalLoopClosureParentId)
+			if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureParentId)
 			{
 				_lastGlobalLoopClosureParentId = 0;
 				_lastGlobalLoopClosureChildId = 0;
 			}
 		}
 		else
+		{
+			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,24 +2169,33 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 			return true;
 		}
 
-		if(type != Link::kVirtualClosure)
+		UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
-		{
-			_memoryChanged = true;
-		}
-		UDEBUG("Add loop closure 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();
+			_lastGlobalLoopClosureParentId = newS->id()>oldS->id()?newS->id():oldS->id();
-			_lastGlobalLoopClosureChildId = oldS->id();
+			_lastGlobalLoopClosureChildId = newS->id()>oldS->id()?oldS->id():newS->id();
 
 			// udpate weights only if the memory is incremental
+			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;
 	}
 	else
@@ -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));
+		fromS->addLink(Link(fromId, toId, type, transform, variance));
-		toS->addLink(Link(toId, fromId, Link::kNeighbor, transform.inverse(), 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);
 	}
 }
 

corelib/src/Rtabmap.cpp

@@ -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
+					if(minDistance == -1 || minDistance > dist[i])
-					   (minDistance == -1 || minDistance > dist[i]))
 					{
 						nearestId = ids[ind[i]];
 						minDistance = dist[i];

corelib/src/util3d.cpp

@@ -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;
+		// Added X2 to make sure that all points are inside the map (when rounded to integer)
-		yMin = min.y-cellSize;
+		xMin = min.x-cellSize*2.0f;
-		float xMax = max.x+cellSize;
+		yMin = min.y-cellSize*2.0f;
-		float yMax = max.y+cellSize;
+		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);
 

guilib/src/DatabaseViewer.cpp

@@ -244,11 +244,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
-					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
+					memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
 				}
 				else
 				{
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
+					memory_->addLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
@@ -257,15 +257,14 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			{
 				if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
 				{
-					memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+					memory_->updateLink(iter->second.from(), iter->second.to(), iter->second.transform(), iter->second.variance());
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
 			// Rejected links
 			for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
 			{
-				memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+				memory_->removeLink(iter->second.to(), iter->second.from());
 			}
 		}
 

guilib/src/MainWindow.cpp

@@ -1418,6 +1418,8 @@ void MainWindow::updateMapCloud(
 			}
 		}
 	}
+	_ui->graphicsView_graphView->update();
+
 	if(!_preferencesDialog->getGridMapShown())
 	{
 		_ui->widget_cloudViewer->removeOccupancyGridMap();