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

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

@@ -91,7 +91,8 @@ void segmentObstaclesFromGround(
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false, bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f); float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0);
template<typename PointT> template<typename PointT>
void segmentObstaclesFromGround( void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -102,7 +103,8 @@ void segmentObstaclesFromGround(
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false, bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f); float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0);
template<typename PointT> template<typename PointT>
void occupancy2DFromCloud3D( 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; float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20; int minClusterSize = 20;
cv::Mat ground, obstacles; 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 // add pose rotation without yaw
float roll, pitch, yaw; float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw); pose.getEulerAngles(roll, pitch, yaw);
voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0)); 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, voxelCloud,
ground, ground,
obstacles, obstacles,