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 biggest index/min/max when Grid/MaxGroundHeight is set (http://official-rtab-map-forum.67519.x6.nabble.com/Comparison-between-realsense-D435-vs-T265-vs-T265-D435-dual-setup-td6456i20.html)

Browse Source
This commit is contained in:
matlabbe
2020-10-18 15:56:13 -04:00
parent a4da1e14b4
commit fbc30042c4
1 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -97,28 +97,35 @@ void segmentObstaclesFromGround(
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface // cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size()) if(clusteredFlatSurfaces.size())
{ {
Eigen::Vector4f min,max; Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
if(maxGroundHeight != 0.0f) if(maxGroundHeight != 0.0f)
{ {
// Search for biggest surface under max ground height // Search for biggest surface under max ground height
size_t points = 0; size_t points = 0;
biggestFlatSurfaceIndex = -1;
for(size_t i=0;i<clusteredFlatSurfaces.size();++i) for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
{ {
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max); pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points) if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
{ {
points = clusteredFlatSurfaces.at(i)->size(); points = clusteredFlatSurfaces.at(i)->size();
biggestFlatSurfaceIndex = i; biggestFlatSurfaceIndex = i;
biggestSurfaceMin = min;
biggestSurfaceMax = max;
} }
} }
} }
else else
{ {
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max); pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);
} }
if(biggestFlatSurfaceIndex>=0)
{
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex); ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
}
if(!ground->empty() && (maxGroundHeight == 0.0f || min[2] < maxGroundHeight)) if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
{ {
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i) for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{ {
@@ -126,7 +133,7 @@ void segmentObstaclesFromGround(
{ {
Eigen::Vector4f centroid(0,0,0,1); Eigen::Vector4f centroid(0,0,0,1);
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid); pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= max[2]) // epsilon if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
{ {
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i)); ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
} }