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

OccupancyGrid: added Grid/Footprint*** parameters, templated segmentCloud() function, added Grid/ClusterRadius parameter. Parameters: fixed getDefaultParameters(group) function to correctly compare groups.

Browse Source
This commit is contained in:
matlabbe
2016-08-30 18:22:29 -04:00
parent 6fe7d5a856
commit c404234635
10 changed files with 522 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <pcl/point_cloud.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
@@ -42,7 +44,22 @@ public:
void parseParameters(const ParametersMap & parameters);
void setCellSize(float cellSize);
float getCellSize() const {return cellSize_;}
void createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const;
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr segmentCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & pose,
const cv::Point3f & viewPoint,
pcl::IndicesPtr & groundIndices, // output cloud indices
pcl::IndicesPtr & obstaclesIndices, // output cloud indices
pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
void createLocalMap(
const Signature & node,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Point3f & viewPoint) const;
void clear();
void addToCache(
@@ -63,6 +80,9 @@ private:
float cloudMaxDepth_;
float cloudMinDepth_;
std::vector<float> roiRatios_;
float footprintLength_;
float footprintWidth_;
float footprintHeight_;
int scanDecimation_;
float cellSize_;
bool occupancyFromCloud_;
@@ -70,6 +90,7 @@ private:
float maxObstacleHeight_;
int normalKSearch_;
float maxGroundAngle_;
float clusterRadius_;
int minClusterSize_;
bool flatObstaclesDetected_;
float minGroundHeight_;
@@ -93,4 +114,6 @@ private:
}
#include <rtabmap/core/impl/OccupancyGrid.hpp>
#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */

8
corelib/include/rtabmap/core/Parameters.h
View File

@@ -467,6 +467,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, DepthMin, float, 0.0, uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, DepthMax, float, 4.0, uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM_STR(Grid, DepthRoiRatios, "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, FootprintLength, float, 0.0, "Footprint length used to filter points over the footprint of the robot.");
RTABMAP_PARAM(Grid, FootprintWidth, float, 0.0, "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
RTABMAP_PARAM(Grid, FootprintHeight, float, 0.0, "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
RTABMAP_PARAM(Grid, ScanDecimation, int, 1, uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, CellSize, float, 0.05, "Resolution of the occupancy grid.");
RTABMAP_PARAM(Grid, MapFrameProjection, bool, false, "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
@@ -475,8 +478,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, MinGroundHeight, float, 0.0, "Minimum ground height (0=disabled).");
RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, MaxGroundAngle, float, 45, uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NormalK, int, 10, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
RTABMAP_PARAM(Grid, MinClusterSize, int, 10, uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
RTABMAP_PARAM(Grid, NormalK, int, 10, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, ClusterRadius, float, 0.1, uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, MinClusterSize, int, 10, uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, true, uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
#ifdef RTABMAP_OCTOMAP
RTABMAP_PARAM(Grid, 3D, bool, true, uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));

162
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp Normal file
View File

@@ -0,0 +1,162 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
const pcl::IndicesPtr & indicesIn,
const Transform & pose,
const cv::Point3f & viewPoint,
pcl::IndicesPtr & groundIndices,
pcl::IndicesPtr & obstaclesIndices,
pcl::IndicesPtr * flatObstacles) const
{
typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
// voxelize to grid cell size
cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
pcl::IndicesPtr indices(new std::vector<int>);
indices->resize(cloud->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
indices->at(i) = i;
}
// add pose rotation without yaw
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
UDEBUG("node.getPose()=%s projMapFrame_=%d", pose.prettyPrint().c_str(), projMapFrame_?1:0);
cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0));
// filter footprint
if(footprintLength_ > 0.0f || footprintWidth_ > 0.0f || footprintHeight_ > 0.0f)
{
indices = util3d::cropBox(
cloud,
indices,
Eigen::Vector4f(
footprintLength_>0.0f?-footprintLength_/2.0f:std::numeric_limits<int>::min(),
footprintWidth_>0.0f&&footprintLength_>0.0f?-footprintWidth_/2.0f:std::numeric_limits<int>::min(),
0,
1),
Eigen::Vector4f(
footprintLength_>0.0f?footprintLength_/2.0f:std::numeric_limits<int>::max(),
footprintWidth_>0.0f&&footprintLength_>0.0f?footprintWidth_/2.0f:std::numeric_limits<int>::max(),
footprintHeight_>0.0f&&footprintLength_>0.0f&&footprintWidth_>0.0f?footprintHeight_:std::numeric_limits<int>::max(),
1),
Transform::getIdentity(),
true);
}
// filter ground/obstacles zone
if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
{
indices = util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
}
if(indices->size())
{
if(normalsSegmentation_)
{
UDEBUG("normalKSearch=%d", normalKSearch_);
UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", clusterRadius_);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
obstaclesIndices,
normalKSearch_,
maxGroundAngle_,
clusterRadius_,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,
flatObstacles,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
}
else
{
UDEBUG("");
// passthrough filter
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
}
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
UDEBUG("");
if(groundIndices->size())
{
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(obstaclesIndices->size())
{
obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(flatObstacles && (*flatObstacles)->size())
{
*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(groundIndices->empty() && obstaclesIndices->empty())
{
UWARN("Cloud (with %d points) is empty after noise "
"filtering. Occupancy grid cannot be "
"created.",
(int)cloud->size());
}
}
}
return cloud;
}
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */

27
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -136,6 +136,33 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
float max,
bool negative = false);
pcl::IndicesPtr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::IndicesPtr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,