i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

icp_odometry: updated how maxLaserScans is set if we receive an organized point cloud

Browse Source
This commit is contained in:
matlabbe
2020-02-21 15:03:22 -05:00
parent 7d0711878e
commit f54e060787
1 changed files with 23 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+23 -2
View File
@@ -402,6 +402,13 @@ private:
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
}
else if(cloudMsg.height > 1 && scanCloudMaxPoints_ != cloudMsg.height * cloudMsg.width)
{
NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
scanCloudMaxPoints_, cloudMsg.width *cloudMsg.height, cloudMsg.width, cloudMsg.height);
scanCloudMaxPoints_ = cloudMsg.width *cloudMsg.height;
}
int maxLaserScans = scanCloudMaxPoints_;
if(containNormals)
{
@@ -410,7 +417,14 @@ private:
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
if(pclScan->height>1)
{
maxLaserScans = pclScan->height * pclScan->width;
}
else
{
maxLaserScans /= scanDownsamplingStep_;
}
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
@@ -428,7 +442,14 @@ private:
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
if(pclScan->height>1)
{
maxLaserScans = pclScan->height * pclScan->width;
}
else
{
maxLaserScans /= scanDownsamplingStep_;
}
}
if(!pclScan->is_dense)
{