Graph Optimization Reverts Added or Cleaned Obstacles in the Occupancy Grid Map

[anath93]

Hi Mathieu,

Sorry to duplicate this from your forum page and thank your for continued and active support but this can be confirmed bug and just been causing issue for long time and did not had chance to dive deeper into this as we had disabled the tags. I know you are very busy, can you please shed light on this when you get a chance. I do not want this to go unnoticed as this will cause issues who ever is trying to use tags.

Also I am at the commit 0.21.10 for humble.

https://easyupload.io/lr5r9e

https://easyupload.io/6fspe0

This is the log screenshot where after seeing tags, which does not contain pose info gets cleared with all other poses and added again from cache where the issue comes. Meaning the changes we had made offline in the editor reverts back to map which was generated during mapping and does not contain the newly edited changes.

Below are my launch files and database.

https://easyupload.io/m/98662i

Currently I applied fix to just clear the Nodes which are not part of graph instead of complete reset where the issues lies somewhere as I cannot figure out. Printed from debug messages when we start to make sure that all the nodes are there which needs to be which is check, and then after graphchanged comes true, logically it looks good as it clears all the nodes, adds the nodes correctly from cache, somewhere there after before publishing things goes wrong. But below clearing method worked, its for loop, can change it to std::map for efficiency but after you confirm.

https://github.com/introlab/rtabmap/blob/master/corelib/src/GlobalMap.cpp#L131

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.
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)
{
	std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
	if(jter != poses.end())
	{
		graphChanged = false;
		UASSERT(!iter->second.isNull() && !jter->second.isNull());
		if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
		{
			graphOptimized = true;
		}
	}
	else
	{
		//extraNodesToClear_.insert(iter->first);
		UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first);
	}
}

**if(graphOptimized || graphChanged)
{
    UDEBUG("Graph updated, selectively clearing outdated nodes...");
    for(auto iter = addedNodes_.begin(); iter != addedNodes_.end();)
    {
	if(poses.find(iter->first) == poses.end()) 
	{
	    iter = addedNodes_.erase(iter); // Remove only nodes not in updated graph
	}
	else 
	{
	    ++iter; // Keep the rest
	}
    }
}**

[matlabbe]

Thank you for the details, I saw the other posts in my mailbox, but I have still a 2 weeks backlog of emails to read. I want to get this one #1451 released soon, I'll check this issue afterwards.

[anath93]

Thank you Mathieu, I appreciate you !