i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

ICP/intensity: Applying fix from #1111

Browse Source
This commit is contained in:
matlabbe
2023-08-13 14:58:15 -07:00
parent 6d36fc1fcd
commit cdd8cd9e34
1 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/icp/libpointmatcher.h
View File

@@ -513,17 +513,29 @@ public:
for (int i = 0; i < pointsCount; ++i) for (int i = 0; i < pointsCount; ++i)
{ {
float minDistance = std::numeric_limits<float>::max(); float minDistance = std::numeric_limits<float>::max();
bool minDistFound = false;
for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k) for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
{ {
float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i))); int matchesIdsCoeff = matches.ids.coeff(k, i);
if (matchesIdsCoeff!=-1)
{
float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matchesIdsCoeff));
if(distIntensity < minDistance) if(distIntensity < minDistance)
{ {
matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i); matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i); matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
minDistance = distIntensity; 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; matches = matchesOrderedByIntensity;
} }
return matches; return matches;