From cdd8cd9e34751a9d2bcbb98854efa9f16a18354c Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 13 Aug 2023 14:58:15 -0700
Subject: [PATCH] ICP/intensity: Applying fix from #1111

---
 corelib/src/icp/libpointmatcher.h | 22 +++++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/corelib/src/icp/libpointmatcher.h b/corelib/src/icp/libpointmatcher.h
index e2d362a6cd..46a5ef0830 100644
--- a/corelib/src/icp/libpointmatcher.h
+++ b/corelib/src/icp/libpointmatcher.h
@@ -513,16 +513,28 @@ struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
 			for (int i = 0; i < pointsCount; ++i)
 			{
 				float minDistance = std::numeric_limits<float>::max();
+				bool minDistFound = false;
 				for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
 				{
-					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
-					if(distIntensity < minDistance)
+					int matchesIdsCoeff = matches.ids.coeff(k, i);
+					if (matchesIdsCoeff!=-1)
 					{
-						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
-						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
-						minDistance = distIntensity;
+						float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matchesIdsCoeff));
+						if(distIntensity < minDistance)
+						{
+							matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
+							matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
+							minDistance = distIntensity;
+							minDistFound = true;
+						}
 					}
 				}
+
+				if (!minDistFound)
+				{
+					matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(0, i);
+					matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(0, i);
+				}
 			}
 			matches = matchesOrderedByIntensity;
 		}