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

Fixed loadScan return 2D cloud for organized PCD

Browse Source
This commit is contained in:
matlabbe
2021-02-10 21:35:09 -05:00
parent 481a140f84
commit 089441a496
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/util3d.cpp
View File

@@ -3295,6 +3295,10 @@ LaserScan loadScan(const std::string & path)
{ {
pcl::io::loadPLYFile(path, *cloud); pcl::io::loadPLYFile(path, *cloud);
} }
if(cloud->height > 1)
{
cloud->is_dense = false;
}
bool is2D = false; bool is2D = false;
if(!cloud->data.empty()) if(!cloud->data.empty())
@@ -3312,22 +3316,19 @@ LaserScan loadScan(const std::string & path)
if(zOffset>=0) if(zOffset>=0)
{ {
is2D = true; is2D = true;
for (uint32_t row = 0; row < (uint32_t)cloud->height; ++row) for (uint32_t row = 0; row < (uint32_t)cloud->height && is2D; ++row)
{ {
const uint8_t* row_data = &cloud->data[row * cloud->row_step]; const uint8_t* row_data = &cloud->data[row * cloud->row_step];
for (uint32_t col = 0; col < (uint32_t)cloud->width; ++col) for (uint32_t col = 0; col < (uint32_t)cloud->width && is2D; ++col)
{ {
const uint8_t* msg_data = row_data + col * cloud->point_step; const uint8_t* msg_data = row_data + col * cloud->point_step;
float z = *(float*)(msg_data + zOffset); float z = *(float*)(msg_data + zOffset);
is2D = z == 0.0f; is2D = z == 0.0f;
if(!is2D)
{
break;
}
} }
} }
} }
} }
return laserScanFromPointCloud(*cloud, true, is2D); return laserScanFromPointCloud(*cloud, true, is2D);
} }
return LaserScan(); return LaserScan();