Pnp multicam refactoring

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 20)
+SET(RTABMAP_PATCH_VERSION 21)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

corelib/include/rtabmap/core/Memory.h

@@ -324,6 +324,7 @@ class RTABMAP_EXP Memory
 	float _laserScanGroundNormalsUp;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
+	bool _invertedReg;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;

corelib/include/rtabmap/core/Parameters.h

@@ -370,6 +370,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
     RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
     RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
+    RTABMAP_PARAM(RGBD, InvertedReg,                  bool, false,  "On loop closure, do registration from the target to reference instead of reference to target.");
     RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
@@ -594,6 +595,7 @@ class RTABMAP_EXP Parameters
 #else
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
+    RTABMAP_PARAM(Vis, PnPMaxVariance,           float, 0.0,    uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
 
     RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");

corelib/include/rtabmap/core/RegistrationVis.h

@@ -82,6 +82,7 @@ class RTABMAP_EXP RegistrationVis : public Registration
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
+	float _PnPMaxVar;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -48,6 +48,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
@@ -63,6 +64,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			cv::Mat * covariance = 0, // mean reproj error if words3B is not set

corelib/src/Memory.cpp

@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
+	_invertedReg(Parameters::defaultRGBDInvertedReg()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -578,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
+	Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
+	if(_invertedReg && _localBundleOnLoopClosure)
+	{
+		UWARN("%s and %s cannot be used at the same time, disabling %s...",
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
+				Parameters::kRGBDInvertedReg().c_str(),
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
+		_localBundleOnLoopClosure = false;
+	}
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -2855,8 +2865,21 @@ Transform Memory::computeTransform(
 		(fromS.getWords().size() && toS.getWords().size()) ||
 		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
-		Signature tmpFrom = fromS;
-		Signature tmpTo = toS;
+		Signature tmpFrom, tmpTo;
+		if(_invertedReg)
+		{
+			tmpFrom = toS;
+			tmpTo = fromS;
+			if(!guess.isNull())
+			{
+				guess = guess.inverse();
+			}
+		}
+		else
+		{
+			tmpFrom = fromS;
+			tmpTo = toS;
+		}
 
 		if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
 		{
@@ -2891,6 +2914,7 @@ Transform Memory::computeTransform(
 				_registrationPipeline->isImageRequired() &&
 			   !_registrationPipeline->isScanRequired() &&
 			   !_registrationPipeline->isUserDataRequired() &&
+			   !_invertedReg &&
 			   !tmpTo.getWordsDescriptors().empty() &&
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
@@ -3115,6 +3139,10 @@ Transform Memory::computeTransform(
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}
+		if(_invertedReg && !transform.isNull())
+		{
+			transform = transform.inverse();
+		}
 	}
 	return transform;
 }

corelib/src/RegistrationVis.cpp

@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
+		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -124,6 +125,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
 	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
+	Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -287,6 +289,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
+	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1580,6 +1583,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
+									_PnPMaxVar,
 									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 									words3B,
 									&covariances[dir],
@@ -1601,6 +1605,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
+									_PnPMaxVar,
 									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 									words3B,
 									&covariances[dir],
@@ -1977,31 +1982,31 @@ Transform RegistrationVis::computeTransformationImpl(
 		if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
 		{
 			cv::Mat pcaData;
-			float cx=0, cy=0, w=0, h=0;
+			std::vector<CameraModel> cameraModelsTo;
+			if(toSignature.sensorData().stereoCameraModels().size())
+			{
+				for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+				{
+					cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+				}
+			}
+			else
+			{
+				cameraModelsTo = toSignature.sensorData().cameraModels();
+			}
 			if(_minInliersDistributionThr > 0)
 			{
-				if((toSignature.sensorData().stereoCameraModels().size() == 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
-					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
+				if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
 				{
-					const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModels().size()?toSignature.sensorData().stereoCameraModels()[0].left():toSignature.sensorData().cameraModels()[0];
-					cx = cameraModel.cx();
-					cy = cameraModel.cy();
-					w = cameraModel.imageWidth();
-					h = cameraModel.imageHeight();
-
-					if(w>0 && h>0)
+					if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
 					{
 						pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
 					}
 					else
 					{
-						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
+						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
 					}
 				}
-				else if(toSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().stereoCameraModels().size() > 1)
-				{
-					UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
-				}
 				else
 				{
 					UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
@@ -2031,12 +2036,14 @@ Transform RegistrationVis::computeTransformationImpl(
 
 				if(!pcaData.empty())
 				{
-					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
+					std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
+					UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
-					ptr[0] = (kpt.pt.x-cx) / w;
-					ptr[1] = (kpt.pt.y-cy) / h;
+					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
+					int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
+					UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
+					ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
+					ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
 				}
 			}
 

corelib/src/Rtabmap.cpp

@@ -1434,32 +1434,45 @@ bool Rtabmap::process(
 				//============================================================
 				// Minimum displacement required to add to Memory
 				//============================================================
-				const std::multimap<int, Link> & links = signature->getLinks();
-				if(links.size() && links.begin()->second.type() == Link::kNeighbor)
+				Transform t;
+
+				if(_memory->isIncremental())
 				{
-					const Signature * s = _memory->getSignature(links.begin()->second.to());
-					UASSERT(s!=0);
-					// don't filter if the new node is not intermediate but previous one is
-					if(signature->getWeight() < 0 || s->getWeight() >= 0)
+					const std::multimap<int, Link> & links = signature->getLinks();
+					if(links.size() && links.begin()->second.type() == Link::kNeighbor)
 					{
-						float x,y,z, roll,pitch,yaw;
-						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-										fabs(y) > _rgbdLinearUpdate ||
-										fabs(z) > _rgbdLinearUpdate ||
-									    (_rgbdAngularUpdate>0.0f && (
-											fabs(roll) > _rgbdAngularUpdate ||
-											fabs(pitch) > _rgbdAngularUpdate ||
-											fabs(yaw) > _rgbdAngularUpdate));
-						if(!isMoving)
+						const Signature * s = _memory->getSignature(links.begin()->second.to());
+						UASSERT(s!=0);
+						// don't filter if the new node is not intermediate but previous one is
+						if(signature->getWeight() < 0 || s->getWeight() >= 0)
 						{
-							// This will disable global loop closure detection, only retrieval will be done.
-							// The location will also be deleted at the end.
-							smallDisplacement = true;
-							UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
+							t = links.begin()->second.transform();
 						}
 					}
 				}
+				else if(!_odomCachePoses.empty())
+				{
+					t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
+				}
+				if(!t.isNull())
+				{
+					float x,y,z, roll,pitch,yaw;
+					t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
+									fabs(y) > _rgbdLinearUpdate ||
+									fabs(z) > _rgbdLinearUpdate ||
+									(_rgbdAngularUpdate>0.0f && (
+										fabs(roll) > _rgbdAngularUpdate ||
+										fabs(pitch) > _rgbdAngularUpdate ||
+										fabs(yaw) > _rgbdAngularUpdate));
+					if(!isMoving)
+					{
+						// This will disable global loop closure detection, only retrieval will be done.
+						// The location will also be deleted at the end.
+						smallDisplacement = true;
+						UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
+					}
+				}
 			}
 			if(odomVelocity.size() == 6)
 			{
@@ -2951,6 +2964,7 @@ bool Rtabmap::process(
 	cv::Mat localizationCovariance;
 	Transform previousMapCorrection;
 	bool rejectedLandmark = false;
+	bool delayedLocalization = false;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
 	UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3441,6 +3455,7 @@ bool Rtabmap::process(
 					else //delayed localization (wait for more than 1 link)
 					{
 						UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
+						delayedLocalization = true;
 						rejectLocalization = true;
 					}
 				}
@@ -3954,10 +3969,21 @@ bool Rtabmap::process(
 			(smallDisplacement || tooFastMovement) &&
 			_loopClosureHypothesis.first == 0 &&
 			lastProximitySpaceClosureId == 0 &&
+			!delayedLocalization &&
 			(rejectedLandmark || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
-		_odomCacheConstraints.erase(signatureRemoved);
+		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
+		{
+			if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
+			{
+				_odomCacheConstraints.erase(iter++);
+			}
+			else
+			{
+				++iter;
+			}
+		}
 	}
 
 	// Pass this point signature should not be used, since it could have been transferred...