i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

API change: segmentObstaclesFromGround() and normalFiltering()

Browse Source
This commit is contained in:
matlabbe
2016-04-12 18:57:04 -04:00
parent fc76e5b8f3
commit 9339b86633
4 changed files with 30 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+11 -7
View File
@@ -23,8 +23,9 @@ void segmentObstaclesFromGround(
const typename pcl::IndicesPtr & indices, const typename pcl::IndicesPtr & indices,
pcl::IndicesPtr & ground, pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles, pcl::IndicesPtr & obstacles,
float normalRadiusSearch, int normalKSearch,
float groundNormalAngle, float groundNormalAngle,
float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles) bool segmentFlatObstacles)
{ {
@@ -39,7 +40,7 @@ void segmentObstaclesFromGround(
indices, indices,
groundNormalAngle, groundNormalAngle,
Eigen::Vector4f(0,0,1,0), Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f, normalKSearch,
Eigen::Vector4f(0,0,100,0)); Eigen::Vector4f(0,0,100,0));
if(segmentFlatObstacles) if(segmentFlatObstacles)
@@ -48,7 +49,7 @@ void segmentObstaclesFromGround(
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters( std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
cloud, cloud,
flatSurfaces, flatSurfaces,
normalRadiusSearch*2.0f, clusterRadius,
minClusterSize, minClusterSize,
std::numeric_limits<int>::max(), std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex); &biggestFlatSurfaceIndex);
@@ -86,7 +87,7 @@ void segmentObstaclesFromGround(
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters( std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud, cloud,
otherStuffIndices, otherStuffIndices,
normalRadiusSearch*2.0f, clusterRadius,
minClusterSize); minClusterSize);
// merge indices // merge indices
@@ -100,8 +101,9 @@ void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground, pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles, pcl::IndicesPtr & obstacles,
float normalRadiusSearch, int normalKSearch,
float groundNormalAngle, float groundNormalAngle,
float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles) bool segmentFlatObstacles)
{ {
@@ -111,8 +113,9 @@ void segmentObstaclesFromGround(
indices, indices,
ground, ground,
obstacles, obstacles,
normalRadiusSearch, normalKSearch,
groundNormalAngle, groundNormalAngle,
clusterRadius,
minClusterSize, minClusterSize,
segmentFlatObstacles); segmentFlatObstacles);
} }
@@ -138,8 +141,9 @@ void occupancy2DFromCloud3D(
indices, indices,
groundIndices, groundIndices,
obstaclesIndices, obstaclesIndices,
cellSize, 20,
groundNormalAngle, groundNormalAngle,
cellSize*2.0f,
minClusterSize); minClusterSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
corelib/include/rtabmap/core/util3d_filtering.h
+6 -6
View File
@@ -344,13 +344,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint); const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering( pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint); const Eigen::Vector4f & viewpoint);
/** /**
@@ -365,7 +365,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
* @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed. * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
* @param angleMax the maximum angle. * @param angleMax the maximum angle.
* @param normal the normal to which each point's normal is compared. * @param normal the normal to which each point's normal is compared.
* @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation). * @param normalKSearch number of neighbor points used for normal estimation (see pcl::NormalEstimation).
* @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation). * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
* @return the indices of the points which respect the normal constraint. * @return the indices of the points which respect the normal constraint.
*/ */
@@ -375,21 +375,21 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint); const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering( pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint); const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering( pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint); const Eigen::Vector4f & viewpoint);
/** /**
corelib/include/rtabmap/core/util3d_mapping.h
+4 -2
View File
@@ -86,8 +86,9 @@ void segmentObstaclesFromGround(
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
pcl::IndicesPtr & ground, pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles, pcl::IndicesPtr & obstacles,
float normalRadiusSearch, int normalKSearch,
float groundNormalAngle, float groundNormalAngle,
float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false); bool segmentFlatObstacles = false);
template<typename PointT> template<typename PointT>
@@ -95,8 +96,9 @@ void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground, pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles, pcl::IndicesPtr & obstacles,
float normalRadiusSearch, int normalKSearch,
float groundNormalAngle, float groundNormalAngle,
float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false); bool segmentFlatObstacles = false);
corelib/src/util3d_filtering.cpp
+9 -9
View File
@@ -1219,21 +1219,21 @@ pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint) const Eigen::Vector4f & viewpoint)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint); return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
} }
pcl::IndicesPtr normalFiltering( pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint) const Eigen::Vector4f & viewpoint)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint); return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
} }
@@ -1243,7 +1243,7 @@ pcl::IndicesPtr normalFiltering(
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint) const Eigen::Vector4f & viewpoint)
{ {
pcl::IndicesPtr output(new std::vector<int>()); pcl::IndicesPtr output(new std::vector<int>());
@@ -1274,7 +1274,7 @@ pcl::IndicesPtr normalFiltering(
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch); ne.setKSearch(normalKSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0) if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{ {
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]); ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1303,7 +1303,7 @@ pcl::IndicesPtr normalFiltering(
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint) const Eigen::Vector4f & viewpoint)
{ {
pcl::IndicesPtr output(new std::vector<int>()); pcl::IndicesPtr output(new std::vector<int>());
@@ -1334,7 +1334,7 @@ pcl::IndicesPtr normalFiltering(
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch); ne.setKSearch (normalKSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0) if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{ {
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]); ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1363,7 +1363,7 @@ pcl::IndicesPtr normalFiltering(
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
float angleMax, float angleMax,
const Eigen::Vector4f & normal, const Eigen::Vector4f & normal,
float radiusSearch, int normalKSearch,
const Eigen::Vector4f & viewpoint) const Eigen::Vector4f & viewpoint)
{ {
pcl::IndicesPtr output(new std::vector<int>()); pcl::IndicesPtr output(new std::vector<int>());