i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

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

@@ -513,16 +513,28 @@ public:
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;
}