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

MainWindow projected map: fixed bad occupancy from wrong normals after voxel filtering. util3d::segmentObstaclesFromGround(): added flatObstacles argument.

Browse Source
This commit is contained in:
matlabbe
2016-05-20 17:25:52 -04:00
parent 4fdaa2b708
commit 971c96f566
3 changed files with 27 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,10 +28,15 @@ void segmentObstaclesFromGround(
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
if(flatObstacles)
{
flatObstacles->reset(new std::vector<int>);
}
if(cloud->size())
{
@@ -75,6 +80,10 @@ void segmentObstaclesFromGround(
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
else if(flatObstacles)
{
*flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
}
}
}
}
@@ -82,6 +91,10 @@ void segmentObstaclesFromGround(
{
// reject ground!
ground.reset(new std::vector<int>);
if(flatObstacles)
{
*flatObstacles = flatSurfaces;
}
}
}
}
@@ -118,7 +131,8 @@ void segmentObstaclesFromGround(
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>(
@@ -131,7 +145,8 @@ void segmentObstaclesFromGround(
clusterRadius,
minClusterSize,
segmentFlatObstacles,
maxGroundHeight);
maxGroundHeight,
flatObstacles);
}
template<typename PointT>

6
corelib/include/rtabmap/core/util3d_mapping.h
View File

@@ -91,7 +91,8 @@ void segmentObstaclesFromGround(
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -102,7 +103,8 @@ void segmentObstaclesFromGround(
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0);
template<typename PointT>
void occupancy2DFromCloud3D(

7
guilib/src/MainWindow.cpp
View File

@@ -2230,14 +2230,17 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
cv::Mat ground, obstacles;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelCloud = util3d::voxelize(cloud, indices, cellSize);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = util3d::voxelize(cloudWithoutNormals, indices, cellSize);
// add pose rotation without yaw
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGBNormal>(
pcl::io::savePCDFile("cloud.pcd", *voxelCloud);
UWARN("saved cloud.pcd");
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
voxelCloud,
ground,
obstacles,