Fixed graph deformation check without landmark's orientation optimized. Added parameter Marker/VarianceOrientationIgnored to be able to tune correctly GTSAM's bearing/range factor variance.

corelib/src/Graph.cpp

@@ -958,12 +958,24 @@ void computeMaxGraphErrors(
 				return;
 			}
 
-			Transform t = t1.inverse()*t2;
+			Transform t;
+			Transform linkT;
+			if(iter->second.from() < 0)
+			{
+				// For landmarks, compare from node to landmark, in case we optimized only marker's position
+				t = t2.inverse()*t1;
+				linkT = iter->second.transform().inverse();
+			}
+			else
+			{
+				t = t1.inverse()*t2;
+				linkT = iter->second.transform();
+			}
 
 			float linearError = uMax3(
-					fabs(iter->second.transform().x() - t.x()),
-					fabs(iter->second.transform().y() - t.y()),
-					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					fabs(linkT.x() - t.x()),
+					fabs(linkT.y() - t.y()),
+					force3DoF?0:fabs(linkT.z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -984,7 +996,7 @@ void computeMaxGraphErrors(
 				float opt_roll,opt_pitch,opt_yaw;
 				float link_roll,link_pitch,link_yaw;
 				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-				iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+				linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
 				float angularError = uMax3(
 						force3DoF?0:fabs(opt_roll - link_roll),
 						force3DoF?0:fabs(opt_pitch - link_pitch),

corelib/src/Memory.cpp

@@ -120,6 +120,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_detectMarkers(Parameters::defaultRGBDMarkerDetection()),
 	_markerLinVariance(Parameters::defaultMarkerVarianceLinear()),
 	_markerAngVariance(Parameters::defaultMarkerVarianceAngular()),
+	_markerOrientationIgnored(Parameters::defaultMarkerVarianceOrientationIgnored()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -615,8 +616,24 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers);
 	Parameters::parse(params, Parameters::kMarkerVarianceLinear(), _markerLinVariance);
 	Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
+	Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
 	Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
 
+	if(_markerAngVariance>=9999)
+	{
+		UWARN("Using directly %s>=9999 to ignore marker orientation is deprecated. Use %s instead and "
+			  "read correctly the description of the new parameter. We will enable %s and set %s to "
+			  "same value than %s (%f) for backward compatibility.", 
+			Parameters::kMarkerVarianceAngular().c_str(),
+			Parameters::kMarkerVarianceOrientationIgnored().c_str(),
+			Parameters::kMarkerVarianceOrientationIgnored().c_str(),
+			Parameters::kMarkerVarianceAngular().c_str(),
+			Parameters::kMarkerVarianceLinear().c_str(),
+			_markerLinVariance);
+			_markerAngVariance = _markerLinVariance;
+			_markerOrientationIgnored = true;
+	}
+
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
@@ -5593,8 +5610,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						continue;
 					}
 					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					if(_markerOrientationIgnored)
+					{
+						covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999; // disable orientation estimation
+						bool isGTSAM = uStr2Int(uValue(parameters_, Parameters::kOptimizerStrategy(), uNumber2Str(Parameters::defaultOptimizerStrategy()))) == Optimizer::kTypeGTSAM;
+						if(!isGTSAM)
+						{
+							covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+						}
+						else if(_registrationPipeline->force3DoF())
+						{
+							// Bearing/Range in 2D, set X as bearing and Y as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,1), cv::Range(0,1)) *= _markerAngVariance;
+							covariance(cv::Range(1,3), cv::Range(1,3)) *= _markerLinVariance;
+						}
+						else
+						{
+							// Bearing/Range in 3D, set X and Y as bearing and Z as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,2), cv::Range(0,2)) *= _markerAngVariance;
+							covariance(cv::Range(2,3), cv::Range(2,3)) *= _markerLinVariance;
+						}
+					}
+					else
+					{
+						covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+						covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					}
 					landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
 				}
 				UDEBUG("Markers detected = %d", (int)markers.size());

corelib/src/Rtabmap.cpp

@@ -1241,7 +1241,7 @@ bool Rtabmap::process(
 	double timeStatsCreation = 0;
 
 	float hypothesisRatio = 0.0f; // Only used for statistics
-	bool rejectedGlobalLoopClosure = false;
+	bool rejectedLoopClosure = false;
 
 	std::map<int, float> rawLikelihood;
 	std::map<int, float> adjustedLikelihood;
@@ -2159,7 +2159,7 @@ bool Rtabmap::process(
 				// Loop closure Threshold
 				if(_highestHypothesis.second >= loopThr)
 				{
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					if(posterior.size() <= 2 && loopThr>0.0f)
 					{
 						// Ignore loop closure if there is only one loop closure hypothesis
@@ -2181,7 +2181,7 @@ bool Rtabmap::process(
 					else
 					{
 						_loopClosureHypothesis = _highestHypothesis;
-						rejectedGlobalLoopClosure = false;
+						rejectedLoopClosure = false;
 					}
 
 					timeHypothesesValidation = timer.ticks();
@@ -2192,7 +2192,7 @@ bool Rtabmap::process(
 					// Used for Precision-Recall computation.
 					// When analyzing logs, it's convenient to know
 					// if the hypothesis would be rejected if T_loop would be lower.
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
 				}
 
@@ -3061,15 +3061,15 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
-				rejectedGlobalLoopClosure = transform.isNull();
-				if(rejectedGlobalLoopClosure)
+				rejectedLoopClosure = transform.isNull();
+				if(rejectedLoopClosure)
 				{
 					UWARN("Rejected loop closure %d -> %d: %s",
 							_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
 				}
 				else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
 				{
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
 							_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
 				}
@@ -3078,7 +3078,7 @@ bool Rtabmap::process(
 					transform = transform.inverse();
 				}
 			}
-			if(!rejectedGlobalLoopClosure)
+			if(!rejectedLoopClosure)
 			{
 				// Make the new one the parent of the old one
 				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
@@ -3086,14 +3086,14 @@ bool Rtabmap::process(
 				loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
 				loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
 				cv::Mat information = getInformation(info.covariance);
-				rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
-				if(!rejectedGlobalLoopClosure)
+				rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
+				if(!rejectedLoopClosure)
 				{
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
 				}
 			}
 
-			if(rejectedGlobalLoopClosure)
+			if(rejectedLoopClosure)
 			{
 				_loopClosureHypothesis.first = 0;
 			}
@@ -3137,7 +3137,7 @@ bool Rtabmap::process(
 				{
 					UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
 					landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
-					rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
+					rejectedLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first));
 				}
 			}
@@ -3180,7 +3180,6 @@ bool Rtabmap::process(
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	Transform previousMapCorrection;
-	bool rejectedLandmark = false;
 	bool delayedLocalization = false;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
@@ -3768,8 +3767,7 @@ bool Rtabmap::process(
 			{
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 		}
 		else
@@ -3804,8 +3802,7 @@ bool Rtabmap::process(
 				updateConstraints = false;
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 			else if(_memory->isIncremental() &&
 			  loopClosureLinksAdded.size() &&
@@ -3915,8 +3912,7 @@ bool Rtabmap::process(
 					updateConstraints = false;
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
-					rejectedGlobalLoopClosure = true;
-					rejectedLandmark = true;
+					rejectedLoopClosure = true;
 				}
 			}
 
@@ -4077,7 +4073,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
 
 			float x,y,z,roll,pitch,yaw;
-			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
+			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLoopClosure && !landmarksDetected.empty()))
 			{
 				if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
 				{
@@ -4179,7 +4175,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
 
 			//Epipolar geometry constraint
-			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedGlobalLoopClosure?1.0f:0);
+			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedLoopClosure?1.0f:0);
 
 			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
 			statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
@@ -4274,7 +4270,7 @@ bool Rtabmap::process(
 		if(_startNewMapOnLoopClosure &&
 			_memory->isIncremental() &&              // only in mapping mode
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 &&      // alone in the current map
-			(landmarksDetected.empty() || rejectedLandmark) &&      // if we re not seeing a landmark from a previous map
+			(landmarksDetected.empty() || rejectedLoopClosure) &&      // if we re not seeing a landmark from a previous map
 			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -4294,7 +4290,7 @@ bool Rtabmap::process(
 		else if((smallDisplacement || tooFastMovement) &&
 				_loopClosureHypothesis.first == 0 &&
 				lastProximitySpaceClosureId == 0 &&
-				(rejectedLandmark || landmarksDetected.empty()) &&
+				(rejectedLoopClosure || landmarksDetected.empty()) &&
 				!addedNewLandmark)
 		{
 			// Don't delete the location if a loop closure is detected
@@ -4314,7 +4310,7 @@ bool Rtabmap::process(
 			_loopClosureHypothesis.first == 0 &&
 			lastProximitySpaceClosureId == 0 &&
 			!delayedLocalization &&
-			(rejectedLandmark || landmarksDetected.empty()))
+			(rejectedLoopClosure || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
 		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
@@ -4722,7 +4718,7 @@ bool Rtabmap::process(
 									refWordsCount,
 									dictionarySize,
 									int(_memory->getWorkingMem().size()),
-									rejectedGlobalLoopClosure?1:0,
+									rejectedLoopClosure?1:0,
 									0,
 									0,
 									int(signaturesRetrieved.size()),