Memory::computeTransform(): removed setting guess to identity if input guess is null

corelib/src/Memory.cpp

@@ -2138,10 +2138,14 @@ Transform Memory::computeTransform(
 			// Remove descriptors, this will avoid recomputation of the correspondences in regVis
 			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			Transform t = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			// set back descriptors
 			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
 			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
+			if(!t.isNull())
+			{
+				guess = t;
+			}
 		}
 
 		if(_reextractLoopClosureFeatures)
@@ -2157,44 +2161,32 @@ Transform Memory::computeTransform(
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 		}
 
-		if(guess.isNull())
+		if(guess.isNull() && !_registrationPipeline->isImageRequired())
-		{
-			if(!_registrationPipeline->isImageRequired())
-			{
-				UDEBUG("");
-				// no visual in the pipeline, make visual registration for guess
-				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-			}
-			else
-			{
-				UDEBUG("");
-				guess.setIdentity();
-			}
-		}
-
-		if(!guess.isNull())
 		{
 			UDEBUG("");
-			transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+			// no visual in the pipeline, make visual registration for guess
+			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+		}
 
-			if(!transform.isNull())
+		transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+
+		if(!transform.isNull())
+		{
+			UDEBUG("");
+			// verify if it is a 180 degree transform, well verify > 90
+			float x,y,z, roll,pitch,yaw;
+			transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			if(fabs(roll) > CV_PI/2 ||
+			   fabs(pitch) > CV_PI/2 ||
+			   fabs(yaw) > CV_PI/2)
 			{
-				UDEBUG("");
+				transform.setNull();
-				// verify if it is a 180 degree transform, well verify > 90
+				std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-				float x,y,z, roll,pitch,yaw;
+						roll, pitch, yaw);
-				transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				UINFO(msg.c_str());
-				if(fabs(roll) > CV_PI/2 ||
+				if(info)
-				   fabs(pitch) > CV_PI/2 ||
-				   fabs(yaw) > CV_PI/2)
 				{
-					transform.setNull();
+					info->rejectedMsg = msg;
-					std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-							roll, pitch, yaw);
-					UINFO(msg.c_str());
-					if(info)
-					{
-						info->rejectedMsg = msg;
-					}
 				}
 			}
 		}

corelib/src/Registration.cpp

@@ -214,7 +214,7 @@ Transform Registration::computeTransformationMod(
 		}
 		else if(!guess.isNull())
 		{
-			// This registration approach failed, continue with the guess for the next registration
+			UDEBUG("This registration approach failed, continue with the guess for the next registration");
 			t = child_->computeTransformationMod(from, to, guess, &info);
 		}
 	}

corelib/src/Rtabmap.cpp

@@ -1017,7 +1017,7 @@ bool Rtabmap::process(
 				//============================================================
 				if(!signature->sensorData().laserScanCompressed().empty())
 				{
-					UINFO("Odometry refining");
+					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
 					RegistrationInfo info;
 					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())