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

DbViewer: fixed empty from scan when refining proximity link by space

Browse Source
This commit is contained in:
matlabbe
2020-06-22 13:07:27 -04:00
parent c5ec4f337b
commit 1ca2c25a10
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
guilib/src/DatabaseViewer.cpp
View File

@@ -6922,10 +6922,14 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
// make sure the current pose is still here
filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
}
Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
fromS->sensorData().uncompressData();
LaserScan fromScan = fromS->sensorData().laserScanRaw();
int maxPoints = fromScan.size();
if(maxPoints == 0)
{
UWARN("From scan %d is empty!", fromS->id());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
@@ -6974,6 +6978,10 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
if(scan.size() > maxPoints)
{
maxPoints = scan.size();
}
}
else
{
UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
}
}
@@ -6999,6 +7007,10 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
else if(assembledToIClouds->size())
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
else
{
UWARN("Assembled scan is empty!");
}
SensorData assembledData;