fixed reset UPlot on rehearsal after small movement detected. GUI: modified color for pose correction (DarkCyan) and small movement (Gray)

corelib/include/rtabmap/core/Statistics.h

@@ -87,6 +87,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Small_movement,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);

corelib/src/Memory.cpp

@@ -3287,7 +3287,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
 		}
 	}
-	return false; // means that the newS can be removed without problem
+	return false;
 }
 
 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const

corelib/src/Rtabmap.cpp

@@ -2222,7 +2222,7 @@ bool Rtabmap::process(
 	if(_loopClosureHypothesis.first || _publishStats)
 	{
 		ULOGGER_INFO("sending stats...");
-		statistics_.setRefImageId(signature->id());
+		statistics_.setRefImageId(_memory->getLastSignatureId()); // Use last id from Memory (in case of rehearsal)
 		if(_loopClosureHypothesis.first != Memory::kIdInvalid)
 		{
 			statistics_.setLoopClosureId(_loopClosureHypothesis.first);
@@ -2284,6 +2284,8 @@ bool Rtabmap::process(
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
+			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
+
 			if(_publishLikelihood || _publishPdf)
 			{
 				// Child count by parent signature on the root of the memory ... for statistics

guilib/src/MainWindow.cpp

@@ -1076,6 +1076,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
 		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
+		bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
 		_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
 		if(rehearsalMerged > 0)
@@ -1088,12 +1089,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 		else if(scanMatchingSuccess)
 		{
-			_ui->imageView_source->setBackgroundColor(Qt::gray);
+			_ui->imageView_source->setBackgroundColor(Qt::darkCyan);
 		}
 		else if(rehearsedSimilarity)
 		{
 			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
 		}
+		else if(smallMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::gray);
+		}
 
 		UDEBUG("time= %d ms", time.restart());
 

guilib/src/utilite/UPlot.cpp

@@ -435,7 +435,7 @@ void UPlotCurve::addValue(float x, float y)
 	{
 		UWARN("New value (%f) added to curve \"%s\" is smaller "
 			  "than the last added (%f). Clearing the curve.",
-				x, this->name().toStdString().c_str(), _items.back()->pos().x());
+				x, this->name().toStdString().c_str(), ((UPlotItem*)_items.back())->data().x());
 		this->clear();
 	}