New rtabmap-reduceGraph CLI tool (#1655)

* New rtabmap-reduceGraph CLI tool

* fixed some edge cases

* Regenerating optimized map if there was one before reducing the graph

* addMoreLoopClosures: refactored how ctrl-c is handled to stop faster when no loop closures are added

* Added kilted status

* Make offline tool always propagate neighbor merged links

* removed a parameter

* fixed disconnected graph

* fixed --help

* Added error log on Kp/NNStrategy not compatible with huge vocabulary. ReduceGraph/DetectMoreLoopClosures: Make sure original parameters are saved back on closing. g2o: fixing optimizer to Levenberg for SBA to avoid [SetJac] infinite jac fatal error.

* exposing neighbor merged ratio parameter to the tool

* show param in log

* refactored detectMoreLoopClosures to ignore too close nodes in terms of neighbor links based on Mem/STMSize parameter. Reduce graph: added direction parameter.

* Simplified: removed ratio parameter, removed recursive reduction. Just don't reduce if a NM link is longer than maxDistance.

* Removed NNStrategy override, as it was still done on closing when we changed back to original params

* DBViewer: show missing links when showing OptimizedPoses in GraphView, fixed clicking on landmark links

* DetectMoreLoopClosures: Added support for min graph distance option in MainWindow and DbViewer

* slight renaming of ROS jobs

* reprocess: added option --params_last

corelib/src/DBDriverSqlite3.cpp

@@ -4419,14 +4419,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
 
 		// Update links part1
-		if(uStrNumCmp(_version, "0.18.3") >= 0)
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
-		}
-		else
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=?;");
-		}
+		query = uFormat("DELETE FROM Link WHERE from_id=?;");
 		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());
 		for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
@@ -4461,6 +4454,12 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				{
 					stepLink(ppStmt, i->second);
 				}
+				// Save landmarks
+				const std::map<int, Link> & landmarks = (*j)->getLandmarks();
+				for(std::map<int, Link>::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i)
+				{
+					stepLink(ppStmt, i->second);
+				}
 			}
 		}
 		// Finalize (delete) the statement

corelib/src/FlannIndex.cpp

@@ -96,7 +96,7 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
 #else
 		UTimer timer;
 		const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
-		std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
+		std::vector<unsigned char> indexData(1024*1024*1024 + headerSizeBytes); // Max 1 GB
         FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
 		long bytes_written = 0;
         if (indexDataPtr) {

corelib/src/Graph.cpp

@@ -2020,19 +2020,21 @@ std::list<std::pair<int, Transform> > computePath(
 		bool lookInDatabase,
 		bool updateNewCosts,
 		float linearVelocity,  // m/sec
-		float angularVelocity) // rad/sec
+		float angularVelocity, // rad/sec
+		bool ignoreDirectLinks) 
 {
 	UASSERT(memory!=0);
 	UASSERT(fromId>=0);
 	UASSERT(toId!=0);
 	std::list<std::pair<int, Transform> > path;
-	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
+	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d",
 			fromId,
 			toId,
 			lookInDatabase?1:0,
 			updateNewCosts?1:0,
 			linearVelocity,
-			angularVelocity);
+			angularVelocity,
+			ignoreDirectLinks?1:0);
 
 	std::multimap<int, Link> allLinks;
 	if(lookInDatabase)
@@ -2110,7 +2112,9 @@ std::list<std::pair<int, Transform> > computePath(
 		}
 		for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			if(iter->second.from() != iter->second.to())
+			if(iter->second.from() != iter->second.to() &&
+			  (!ignoreDirectLinks || 
+				(!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId))))
 			{
 				Transform nextPose = currentNode->pose()*iter->second.transform();
 				float cost = 0.0f;
@@ -2396,26 +2400,15 @@ std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const st
 
 
 float computePathLength(
-		const std::vector<std::pair<int, Transform> > & path,
-		unsigned int fromIndex,
-		unsigned int toIndex)
+		const std::vector<std::pair<int, Transform> > & path)
 {
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		UASSERT(fromIndex  < path.size() && toIndex < path.size() && fromIndex <= toIndex);
-		if(fromIndex >= toIndex)
+		for(unsigned int i=0; i<path.size()-1; ++i)
 		{
-			toIndex = (unsigned int)path.size()-1;
+			length+=path[i].second.getDistance(path[i+1].second);
 		}
-		float x=0, y=0, z=0;
-		for(unsigned int i=fromIndex; i<toIndex-1; ++i)
-		{
-			x += fabs(path[i].second.x() - path[i+1].second.x());
-			y += fabs(path[i].second.y() - path[i+1].second.y());
-			z += fabs(path[i].second.z() - path[i+1].second.z());
-		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }
@@ -2426,19 +2419,15 @@ float computePathLength(
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		float x=0, y=0, z=0;
 		std::map<int, Transform>::const_iterator iter=path.begin();
 		Transform previousPose = iter->second;
 		++iter;
 		for(; iter!=path.end(); ++iter)
 		{
 			const Transform & currentPose = iter->second;
-			x += fabs(previousPose.x() - currentPose.x());
-			y += fabs(previousPose.y() - currentPose.y());
-			z += fabs(previousPose.z() - currentPose.z());
+			length+=previousPose.getDistance(currentPose);
 			previousPose = currentPose;
 		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }

corelib/src/Memory.cpp

@@ -1288,114 +1288,180 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	}
 }
 
-void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
+bool Memory::canBeReduced(const Link & link, float maxDistance, int direction)
 {
-	UDEBUG("Inserting node %d from STM in WM...", id);
-	UASSERT(_stMem.find(id) != _stMem.end());
+	return  link.to() != link.from() &&
+			link.type() != Link::kNeighbor &&
+			link.type() != Link::kNeighborMerged &&
+			link.userDataCompressed().empty() &&
+			link.type() != Link::kUndef &&
+			link.type() != Link::kVirtualClosure &&
+			(maxDistance == 0.0f || link.transform().getNorm() < maxDistance) &&
+			(direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from()));
+}
+
+int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction)
+{
+	UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)",
+		id, maxDistance, keepLinkedInDb?"true":"false", direction);
 	Signature * s = this->_getSignature(id);
-	UASSERT(s!=0);
-
-	if(_reduceGraph)
+	if(s==0)
 	{
-		bool merge = false;
-		const std::multimap<int, Link> & links = s->getLinks();
-		std::map<int, Link> neighbors;
-		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-		{
-			if(!merge)
-			{
-				merge = iter->second.to() < s->id() && // should be a parent->child link
-						iter->second.to() != iter->second.from() &&
-						iter->second.type() != Link::kNeighbor &&
-						iter->second.type() != Link::kNeighborMerged &&
-						iter->second.userDataCompressed().empty() &&
-						iter->second.type() != Link::kUndef &&
-						iter->second.type() != Link::kVirtualClosure;
-				if(merge)
-				{
-					UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
-					if(reducedTo)
-					{
-						*reducedTo = iter->second.to();
-					}
-				}
+		UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
+		return 0;
+	}
 
-			}
-			if(iter->second.type() == Link::kNeighbor)
+	if(!s->getLabel().empty())
+	{
+		// We currently not remove nodes with labels
+		return 0;
+	}
+
+	std::multimap<int, Link> links = s->getLinks();
+	std::map<int, Link> neighbors;
+	int reducedTo = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(canBeReduced(iter->second, maxDistance, direction))
+		{
+			float distance = iter->second.transform().getNorm();
+			reducedTo = iter->second.to();
+			UDEBUG("Reduce %d to %d (distance=%f)",
+				s->id(), iter->second.to(), distance);
+		}
+		
+		if(iter->second.type() == Link::kNeighbor)
+		{
+			neighbors.insert(*iter);
+		}
+	}
+	if(reducedTo>0)
+	{
+		if(maxDistance > 0.0f)
+		{
+			// Only reduce if all neighbor merged links are also below maxDistance
+			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
-				neighbors.insert(*iter);
+				if( iter->second.type() == Link::kNeighborMerged &&
+					iter->second.transform().getNorm() > maxDistance)
+				{
+					return 0;
+				}
 			}
 		}
-		if(merge)
+
+		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
-			if(s->getLabel().empty())
+			Signature * sTo = this->_getSignature(iter->first);
+			if(sTo->id()!=s->id()) // Not Prior/Gravity links...
 			{
-				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
+				sTo->removeLink(s->id());
+				if(iter->second.type() != Link::kNeighbor &&
+					iter->second.type() != Link::kUndef)
 				{
-					Signature * sTo = this->_getSignature(iter->first);
-					if(sTo->id()!=s->id()) // Not Prior/Gravity links...
+					if(iter->second.type() == Link::kNeighborMerged)
 					{
-						UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
-						sTo->removeLink(s->id());
-						if(iter->second.type() != Link::kNeighbor &&
-						   iter->second.type() != Link::kNeighborMerged &&
-						   iter->second.type() != Link::kUndef)
+						s->removeLink(sTo->id());
+						if(maxDistance == 0.0f)
 						{
-							// link to all neighbors
-							for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+							// online graph reduction, always skip these links
+							continue;
+						}
+					}
+					// link to all neighbors
+					for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+					{
+						if(!sTo->hasLink(jter->second.to()))
+						{
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
+							UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
+								iter->second.to(), iter->second.from(), iter->second.type(),
+								jter->second.from(), jter->second.to(), jter->second.type(),
+								l.from(), l.to(), l.type(), sTo->id(), l.to());
+							sTo->addLink(l);
+							Signature * sB = this->_getSignature(l.to());
+							UASSERT(sB!=0);
+							UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str());
+							sB->addLink(l.inverse());
+						}
+					}
+					// link to all landmarks
+					for(std::map<int, Link>::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter)
+					{
+						if(!uContains(sTo->getLandmarks(), jter->first))
+						{
+							UDEBUG("Move landmark observation %d from %d to %d",
+									jter->first, s->id(), sTo->id());
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									jter->second.type());
+							sTo->addLandmark(l);
+							// Update landmark index
+							std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(jter->first);
+							if(nter!=_landmarksIndex.end())
 							{
-								if(!sTo->hasLink(jter->second.to()))
-								{
-									UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
-											iter->second.from(), iter->second.to(), iter->second.type(),
-											jter->second.from(), jter->second.to(), jter->second.type());
-									Link l = iter->second.inverse().merge(
-											jter->second,
-											iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
-									sTo->addLink(l);
-									Signature * sB = this->_getSignature(l.to());
-									UASSERT(sB!=0);
-									UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
-									sB->addLink(l.inverse());
-								}
+								nter->second.insert(sTo->id());
+							}
+							else
+							{
+								std::set<int> tmp;
+								tmp.insert(sTo->id());
+								_landmarksIndex.insert(std::make_pair(jter->first, tmp));
 							}
 						}
 					}
 				}
+			}
+		}
 
-				//remove neighbor links
-				std::multimap<int, Link> linksCopy = links;
-				for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
+		this->moveToTrash(s, keepLinkedInDb);
+		s = 0;
+		_linksChanged = true;
+		_memoryChanged = true;
+	}
+	return reducedTo;
+}
+
+void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
+{
+	UDEBUG("Inserting node %d from STM in WM...", id);
+	UASSERT(_stMem.find(id) != _stMem.end());
+	int reducedId = 0;
+	if(_reduceGraph)
+	{
+		Signature * s = this->_getSignature(id);
+		UASSERT(s!=0);
+		std::multimap<int, Link> links = s->getLinks();
+		// Setting true to make sure we save all visual
+		// words that could be referenced in a previously
+		// transferred node in LTM (#979)
+		reducedId = reduceNode(s->id(), 0, true);
+		if(reducedToOut) {
+			*reducedToOut = reducedId;
+		}
+		if(reducedId>0)
+		{
+			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kNeighbor)
 				{
-					if(iter->second.type() == Link::kNeighborMerged)
+					if(_lastGlobalLoopClosureId == s->id())
 					{
-						// Removing only merged neighbor links, we keep original neighbor 
-						// links to be able to reprocess databases with correct odometry covariance.
-						s->removeLink(iter->first);
-					}
-					if(iter->second.type() == Link::kNeighbor)
-					{
-						if(_lastGlobalLoopClosureId == s->id())
-						{
-							_lastGlobalLoopClosureId = iter->first;
-						}
+						_lastGlobalLoopClosureId = iter->first;
 					}
 				}
-
-				// Setting true to make sure we save all visual
-				// words that could be referenced in a previously
-				// transferred node in LTM (#979)
-				this->moveToTrash(s, true);
-				s = 0;
 			}
 		}
 	}
-	if(s != 0)
+	if(reducedId == 0)
 	{
 		_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
 		_stMem.erase(*_stMem.begin());
 	}
-	// else already removed from STM/WM in moveToTrash()
+	// else already removed from STM/WM in reduceNode()
 }
 
 const Signature * Memory::getSignature(int id) const
@@ -2610,9 +2676,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		// If not saved to database
 		if(!keepLinkedToGraph)
 		{
-			UASSERT_MSG(this->isInSTM(s->id()),
+			UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()),
 						uFormat("Deleting location (%d) outside the "
-								"STM is not implemented!", s->id()).c_str());
+								"WM/STM is not implemented! STM size=%ld WM size=%ld",
+								s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str());
 			const std::multimap<int, Link> & links = s->getLinks();
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -2623,7 +2690,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					UASSERT_MSG(sTo!=0,
 								uFormat("A neighbor (%d) of the deleted location %d is "
 										"not found in WM/STM! Are you deleting a location "
-										"outside the STM?", iter->first, s->id()).c_str());
+										"outside the WM/STM?", iter->first, s->id()).c_str());
 
 					if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
 					{
@@ -2633,7 +2700,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					}
 
 					// child
-					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
+					if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0)
 					{
 						sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
 					}

corelib/src/Optimizer.cpp

@@ -217,6 +217,10 @@ LinkIdKey(int id, Link::Type type) :
 		{
 			return true;
 		}
+		else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
+		{
+			return false;
+		}
 		else
 		{
 			// normal link, sort by smallest to largest id
@@ -256,7 +260,7 @@ void Optimizer::getConnectedGraph(
 		}
 	}
 
-	while(nextPoses.size())
+	while(!nextPoses.empty())
 	{
 		// Fill up all nodes before landmarks
 		// For nodes, fill up all neightbor nodes before loop closure ones

corelib/src/Rtabmap.cpp

@@ -5731,6 +5731,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 		if(toFromMapId >=0)
 		{
+			size_t clustersBefore = clusters.size();
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
 			{
 				int mapId = uValue(mapIds, iter->first, 0);
@@ -5742,7 +5743,7 @@ int Rtabmap::detectMoreLoopClosures(
 					++iter;
 				}
 			}
-			UINFO("Looking for more loop closures: filtered %ld clusters for map session %d.", clusters.size(), toFromMapId);
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId);
 			if(clusters.empty())
 			{
 				UERROR("No clusters belong to mapId %d, aborting.", toFromMapId);
@@ -5750,6 +5751,33 @@ int Rtabmap::detectMoreLoopClosures(
 			}
 		}
 
+		if(_memory->getMaxStMemSize() > 1)
+		{
+			size_t clustersBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				if(abs(iter->first - iter->second) < _memory->getMaxStMemSize())
+				{
+					iter = clusters.erase(iter);
+				}
+				else
+				{
+					// compute path to know how far we are in terms of graph length
+					std::map<int, int> ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true);
+					if(ids.find(iter->second) != ids.end())
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).",
+				clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize());
+		}
+
 		int i=0;
 		std::set<int> addedLinks;
 		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)

corelib/src/VWDictionary.cpp

@@ -573,6 +573,15 @@ void VWDictionary::update()
 				_removedIndexedWords.size() == 0 &&
 				_visualWords.size())
 		{
+			const int IMGIDX_SHIFT = 18;
+    		const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp
+			if(_dataTree.rows >= IMGIDX_ONE)
+			{
+				UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
+					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
+					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
+			}
+
 			//just add not indexed words
 			int i = _dataTree.rows;
 			if(!_dataTree.empty()) {
@@ -1006,7 +1015,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
-		UDEBUG("newPts.total()=%d ", descriptors.rows);
+		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);
 
 		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 		{

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1560,7 +1560,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 #endif // RTABMAP_ORB_SLAM
 
 #ifndef RTABMAP_ORB_SLAM
-		if(optimizer_ == 1)
+		// ISSUE: It seems the fatal error
+		//        "[SetJac] infinite jac" happens relatively
+		//        easily with GaussNewton on SBA problem,
+		//        ignore optimizer_ and always use Levenberg for SBA.
+		// TODO: Note that g2o/RobustKernelDelta parameter could be
+		//       potentially tuned to avoid that error with GaussNewton.
+		if(0)//optimizer_ == 1)
 		{
 #ifdef RTABMAP_G2O_CPP11
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
@@ -2018,7 +2024,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			if(uIsNan(chi2))
 			{
-				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).",
+						Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_);
 				return optimizedPoses;
 			}
 			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
@@ -2052,15 +2059,18 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif
 
-						cv::Point3f pt3d;
+						int id=-1;
 						if((*iter)->vertex(0)->id() > negVertexOffset)
 						{
-							pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
+							id = negVertexOffset - (*iter)->vertex(0)->id();
 						}
 						else
 						{
-							pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+							id = (*iter)->vertex(0)->id() - stepVertexId;
 						}
+						UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
+							id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
+						cv::Point3f pt3d = points3DMap.at(id);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
 
 						if(outliers)