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

Update downsampling filtering approach for ring-like lidars like the ouster

Browse Source
This commit is contained in:
matlabbe
2020-02-21 15:01:31 -05:00
parent e7a7ab8031
commit 352507cb1c
1 changed files with 54 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
corelib/src/util3d_filtering.cpp
View File

@@ -393,12 +393,61 @@ typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
}
else
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
if(cloud->height > 1 && cloud->height < cloud->width/4)
{
(*output)[oi++] = cloud->at(i);
// Assuming ouster point cloud (e.g, 2048x64),
// for which the lower dimension is the number of rings.
// Downsample each ring by the step.
// Example data packed:
// <ringA-1, ringB-1, ringC-1, ringD-1;
// ringA-2, ringB-2, ringC-2, ringD-2;
// ringA-3, ringB-3, ringC-3, ringD-3;
// ringA-4, ringB-4, ringC-4, ringD-4>
unsigned int rings = cloud->height<cloud->width?cloud->height:cloud->width;
unsigned int pts = cloud->height>cloud->width?cloud->height:cloud->width;
unsigned int finalSize = rings * pts/step;
output->resize(finalSize);
output->width = rings;
output->height = pts/step;
for(unsigned int j=0; j<rings; ++j)
{
for(unsigned int i=0; i<output->height; ++i)
{
(*output)[i*rings + j] = cloud->at(i*step*rings + j);
}
}
}
else if(cloud->height > 1)
{
// assume depth image (e.g., 640x480), downsample like an image
UASSERT_MSG(cloud->height % step == 0 && cloud->width % step == 0,
uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
step, cloud->width, cloud->height).c_str());
int finalSize = cloud->height/step * cloud->width/step;
output->resize(finalSize);
output->width = cloud->width/step;
output->height = cloud->height/step;
for(unsigned int j=0; j<output->height; ++j)
{
for(unsigned int i=0; i<output->width; ++i)
{
output->at(j*output->width + i) = cloud->at(j*output->width*step + i*step);
}
}
}
else
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
}
return output;