Memory: compute scan max range if not set on signature creation.

corelib/src/Memory.cpp

@@ -2485,9 +2485,23 @@ Transform Memory::computeIcpTransformMulti(
 	LaserScan fromScan;
 	fromS->sensorData().uncompressData(0, 0, &fromScan);
 
+	Signature * toS = _getSignature(toId);
+	LaserScan toScan;
+	toS->sensorData().uncompressData(0, 0, &toScan);
+
 	Transform t;
-	if(!fromScan.isEmpty())
+	if(!fromScan.isEmpty() && !toScan.isEmpty())
 	{
+		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
+		float guessNorm = guess.getNorm();
+		if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
+			guessNorm > fromScan.maxRange() + toScan.maxRange())
+		{
+			// stop right known,it is impossible that scans overlay.
+			UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
+			return t;
+		}
+
 		// Create a fake signature with all scans merged in oldId referential
 		SensorData assembledData;
 		Transform toPoseInv = poses.at(toId).inverse();
@@ -2578,7 +2592,6 @@ Transform Memory::computeIcpTransformMulti(
 					fromScan.format(),
 					fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
-		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
 		t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
 	}
 
@@ -3825,6 +3838,33 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
 	{
+		if(laserScan.maxRange() == 0.0f)
+		{
+			bool id2d = laserScan.is2d();
+			float maxRange = 0.0f;
+			for(int i=0; i<laserScan.size(); ++i)
+			{
+				const float * ptr = laserScan.data().ptr<float>(0, i);
+				float r;
+				if(id2d)
+				{
+					r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
+				}
+				else
+				{
+					r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
+				}
+				if(r>maxRange)
+				{
+					maxRange = r;
+				}
+			}
+			if(maxRange > 0.0f)
+			{
+				laserScan=LaserScan(laserScan.data(), laserScan.maxPoints(), sqrt(maxRange), laserScan.format(), laserScan.localTransform());
+			}
+		}
+
 		laserScan = util3d::commonFiltering(laserScan,
 				_laserScanDownsampleStepSize,
 				0,

corelib/src/RegistrationIcp.cpp

@@ -604,7 +604,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 						}
 						catch(const std::exception & e)
 						{
-							UWARN("libpointmatcher has failed: %s", e.what());
+							msg = uFormat("libpointmatcher has failed: %s", e.what());
 						}
 					}
 					else
@@ -827,7 +827,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 								}
 								catch(const std::exception & e)
 								{
-									UWARN("libpointmatcher has failed: %s", e.what());
+									msg = uFormat("libpointmatcher has failed: %s", e.what());
 								}
 							}
 							else
@@ -965,7 +965,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 						}
 						catch(const std::exception & e)
 						{
-							UWARN("libpointmatcher has failed: %s", e.what());
+							msg = uFormat("libpointmatcher has failed: %s", e.what());
 						}
 					}
 					else
@@ -1112,8 +1112,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 			}
 			else
 			{
-				msg = uFormat("Cannot compute transform (converged=%s var=%f)",
+				if(msg.empty())
-						hasConverged?"true":"false", variance);
+				{
+					msg = uFormat("Cannot compute transform (converged=%s var=%f)",
+							hasConverged?"true":"false", variance);
+				}
 				UINFO(msg.c_str());
 			}
 		}

corelib/src/util3d_filtering.cpp

@@ -99,23 +99,27 @@ LaserScan commonFiltering(
 			for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
 			{
 				const float * ptr = scan.data().ptr<float>(0, i);
-				float r;
-				if(is2d)
-				{
-					r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
-				}
-				else
-				{
-					r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
-				}
 
-				if(rangeMin > 0.0f && r < rangeMinSqrd)
+				if(rangeMin>0.0f || rangeMax>0.0f)
 				{
-					continue;
+					float r;
-				}
+					if(is2d)
-				if(rangeMax > 0.0f && r > rangeMaxSqrd)
+					{
-				{
+						r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
-					continue;
+					}
+					else
+					{
+						r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
+					}
+
+					if(rangeMin > 0.0f && r < rangeMinSqrd)
+					{
+						continue;
+					}
+					if(rangeMax > 0.0f && r > rangeMaxSqrd)
+					{
+						continue;
+					}
 				}
 
 				cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
@@ -123,7 +127,7 @@ LaserScan commonFiltering(
 			}
 			int previousSize = scan.size();
 			int scanMaxPtsTmp = scan.maxPoints();
-			scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, scan.maxRange(), scan.format(), scan.localTransform());
+			scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
 			UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
 		}