diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index ee0a2552..51c59c74 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -1052,7 +1052,7 @@ bool Rtabmap::process(
 
 		// Update optimizedPoses with the newly added node
 		Transform newPose;
-		if(signature->getLinks().size() == 1)
+		if(signature->getLinks().size() == 1 && !smallDisplacement)
 		{
 			int oldId = signature->getLinks().begin()->first;
 			const Signature * oldS = _memory->getSignature(oldId);
@@ -1112,7 +1112,10 @@ bool Rtabmap::process(
 			_mapCorrection = newPose * signature->getPose().inverse();
 			if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
 			{
-				UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
+				UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
+						_mapCorrection.prettyPrint().c_str(),
+						newPose.prettyPrint().c_str(),
+						signature->getPose().prettyPrint().c_str());
 			}
 		}
 		else
@@ -2348,9 +2351,9 @@ bool Rtabmap::process(
 
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;
-
+
 	timeMemoryCleanup = timer.ticks();
-	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
+	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
 
 
 
@@ -2518,7 +2521,7 @@ bool Rtabmap::process(
 									timeLocalTimeDetection,
 									timeLocalSpaceDetection,
 									timeMapOptimization);
-		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
 									_loopClosureHypothesis.first,
 									_highestHypothesis.first,
 									(int)signaturesRemoved.size(),
@@ -3060,8 +3063,8 @@ void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		bool optimized,
-		bool global,
-		std::map<int, Signature> * signatures)
+		bool global,
+		std::map<int, Signature> * signatures)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -3083,8 +3086,8 @@ void Rtabmap::getGraph(
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
-
-		if(signatures)
+
+		if(signatures)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -3100,7 +3103,7 @@ void Rtabmap::getGraph(
 							weight,
 							stamp,
 							label,
-							odomPose)));
+							odomPose)));
 			}
 		}
 	}
@@ -3243,7 +3246,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
 			links.insert(std::make_pair(jter->second.from(), jter->second.to()));
 			links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <->
 		}
-	}
+	}
 	UINFO("Time getting links = %fs", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp
index 0859155a..eedbe454 100644
--- a/corelib/src/VWDictionary.cpp
+++ b/corelib/src/VWDictionary.cpp
@@ -1193,17 +1193,20 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			}
 
 			// not indexed..
-			for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
+			if(matchesNotIndexed.size())
 			{
-				float d = matchesNotIndexed.at(i).at(j).distance;
-				int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
-				if(d >= 0.0f && id > 0)
+				for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
 				{
-					fullResults.insert(std::pair<float, int>(d, id));
-				}
-				else
-				{
-					break;
+					float d = matchesNotIndexed.at(i).at(j).distance;
+					int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
+					if(d >= 0.0f && id > 0)
+					{
+						fullResults.insert(std::pair<float, int>(d, id));
+					}
+					else
+					{
+						break;
+					}
 				}
 			}