i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

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,