Statistics: added OdomCache data for debugging

corelib/include/rtabmap/core/Statistics.h

@@ -147,6 +147,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Odom_cache_poses,);
+	RTABMAP_STATS(Memory, Odom_cache_links,);
 	RTABMAP_STATS(Memory, Small_movement,);
 	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
@@ -254,6 +256,8 @@ public:
 	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
 	void setWmState(const std::vector<int> & state) {_wmState = state;}
+	void setOdomCachePoses(const std::map<int, Transform> & poses) {_odomCachePoses = poses;}
+	void setOdomCacheConstraints(const std::multimap<int, Link> & constraints) {_odomCacheConstraints = constraints;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -281,6 +285,8 @@ public:
 	int currentGoalId() const {return _currentGoalId;}
 	const std::map<int, int> & reducedIds() const {return _reducedIds;}
 	const std::vector<int> & wmState() const {return _wmState;}
+	const std::map<int, Transform> & odomCachePoses() const {return _odomCachePoses;}
+	const std::multimap<int, Link> & odomCacheConstraints() const {return _odomCacheConstraints;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -316,6 +322,9 @@ private:
 
 	std::vector<int> _wmState;
 
+	std::map<int, Transform> _odomCachePoses;
+	std::multimap<int, Link> _odomCacheConstraints;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/src/Rtabmap.cpp

@@ -3900,6 +3900,11 @@ bool Rtabmap::process(
 
 		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
 
+		statistics_.setOdomCachePoses(_odomCachePoses);
+		statistics_.setOdomCacheConstraints(_odomCacheConstraints);
+		statistics_.addStatistic(Statistics::kMemoryOdom_cache_poses(), _odomCachePoses.size());
+		statistics_.addStatistic(Statistics::kMemoryOdom_cache_links(), _odomCacheConstraints.size());
+
 		if(_computeRMSE && _memory->getGroundTruths().size())
 		{
 			UDEBUG("Computing RMSE...");

guilib/include/rtabmap/gui/MainWindow.h

@@ -262,6 +262,8 @@ private:
 			const std::map<int, int> & mapIds,
 			const std::map<int, std::string> & labels,
 			const std::map<int, Transform> & groundTruths,
+			const std::map<int, Transform> & odomCachePoses = std::map<int, Transform>(),
+			const std::multimap<int, Link> & odomCacheConstraints = std::multimap<int, Link>(),
 			bool verboseProgress = false,
 			std::map<std::string, float> * stats = 0);
 	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);

guilib/src/MainWindow.cpp

@@ -2412,6 +2412,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					mapIds,
 					labels,
 					groundTruth,
+					stat.odomCachePoses(),
+					stat.odomCacheConstraints(),
 					false,
 					&updateCloudSats);
 
@@ -2531,6 +2533,8 @@ void MainWindow::updateMapCloud(
 		const std::map<int, int> & mapIdsIn,
 		const std::map<int, std::string> & labels,
 		const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
+		const std::map<int, Transform> & odomCachePoses,
+		const std::multimap<int, Link> & odomCacheConstraints,
 		bool verboseProgress,
 		std::map<std::string, float> * stats)
 {
@@ -2950,12 +2954,24 @@ void MainWindow::updateMapCloud(
 		mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
 	}
 
+	std::map<int, Transform> posesWithOdomCache;
+
+	if(_ui->graphicsView_graphView->isVisible() ||
+	   ((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size()))
+	{
+		posesWithOdomCache = posesIn;
+		for(std::map<int, Transform>::const_iterator iter=odomCachePoses.begin(); iter!=odomCachePoses.end(); ++iter)
+		{
+			posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second));
+		}
+	}
+
 	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
 	{
 		UTimer timerGraph;
 		// Find all graphs
 		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
-		for(std::map<int, Transform>::iterator iter=_currentPosesMap.lower_bound(1); iter!=_currentPosesMap.end(); ++iter)
+		for(std::map<int, Transform>::iterator iter=posesWithOdomCache.lower_bound(1); iter!=posesWithOdomCache.end(); ++iter)
 		{
 			int mapId = uValue(_currentMapIds, iter->first, -1);
 
@@ -3045,7 +3061,7 @@ void MainWindow::updateMapCloud(
 				{
 					int id = std::atoi(splitted.back().c_str());
 					if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
-						_currentPosesMap.find(id) == _currentPosesMap.end())
+						posesWithOdomCache.find(id) == posesWithOdomCache.end())
 					{
 						_cloudViewer->removeFrustum(iter.key());
 					}
@@ -3156,7 +3172,10 @@ void MainWindow::updateMapCloud(
 	// Update occupancy grid map in 3D map view and graph view
 	if(_ui->graphicsView_graphView->isVisible())
 	{
-		_ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
+		std::multimap<int, Link> constraintsWithOdomCache;
+		constraintsWithOdomCache = constraints;
+		constraintsWithOdomCache.insert(odomCacheConstraints.begin(), odomCacheConstraints.end());
+		_ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn);
 		if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
 		{
 			std::map<int, Transform> gtPoses = _currentGTPosesMap;
@@ -4440,7 +4459,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_progressCanceled = false;
 			QApplication::processEvents();
 			std::map<int, Transform> poses = event.getPoses();
-			this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
+			this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, std::map<int, Transform>(), std::multimap<int, Link>(), true);
 
 			if( _ui->graphicsView_graphView->isVisible() &&
 			    _preferencesDialog->isWordsCountGraphView() &&
@@ -6618,6 +6637,8 @@ void MainWindow::postProcessing(
 			std::map<int, int>(_currentMapIds),
 			std::map<int, std::string>(_currentLabels),
 			std::map<int, Transform>(_currentGTPosesMap),
+			std::map<int, Transform>(),
+			std::multimap<int, Link>(),
 			false);
 	_progressDialog->appendText(tr("Updating map... done!"));