i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

OccupancyGrid: updated parameters check

Browse Source
This commit is contained in:
matlabbe
2017-08-04 11:41:50 -04:00
parent d8a6ed4ba6
commit 75854fc026
3 changed files with 28 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
+3 -1
View File
@@ -121,7 +121,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
{ {
UDEBUG(""); UDEBUG("");
// passthrough filter // passthrough filter
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_); groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
maxGroundHeight_!=0.0f?maxGroundHeight_:std::numeric_limits<int>::max());
pcl::IndicesPtr notObstacles = groundIndices; pcl::IndicesPtr notObstacles = groundIndices;
if(indices->size()) if(indices->size())
corelib/src/OccupancyGrid.cpp
+20 -20
View File
@@ -98,26 +98,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_); Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_); Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_); Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
if(maxGroundHeight_ != 0.0f &&
maxObstacleHeight_ != 0.0f &&
maxObstacleHeight_ < maxGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str());
maxObstacleHeight_ = 0;
}
if(maxGroundHeight_ != 0.0f &&
minGroundHeight_ != 0.0f &&
maxGroundHeight_ < minGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMinGroundHeight().c_str(),
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMinGroundHeight().c_str());
minGroundHeight_ = 0;
}
Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_); Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_)) if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
{ {
@@ -182,6 +162,26 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::kGridMaxGroundHeight().c_str(), cellSize_); Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
maxGroundHeight_ = cellSize_; maxGroundHeight_ = cellSize_;
} }
if(maxGroundHeight_ != 0.0f &&
maxObstacleHeight_ != 0.0f &&
maxObstacleHeight_ < maxGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str());
maxObstacleHeight_ = 0;
}
if(maxGroundHeight_ != 0.0f &&
minGroundHeight_ != 0.0f &&
maxGroundHeight_ < minGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMinGroundHeight().c_str(),
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMinGroundHeight().c_str());
minGroundHeight_ = 0;
}
} }
void OccupancyGrid::setCellSize(float cellSize) void OccupancyGrid::setCellSize(float cellSize)
corelib/src/util3d_filtering.cpp
+5 -5
View File
@@ -274,7 +274,7 @@ pcl::IndicesPtr passThrough(
float max, float max,
bool negative) bool negative)
{ {
UASSERT(max > min); UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::IndicesPtr output(new std::vector<int>); pcl::IndicesPtr output(new std::vector<int>);
@@ -295,7 +295,7 @@ pcl::IndicesPtr passThrough(
float max, float max,
bool negative) bool negative)
{ {
UASSERT(max > min); UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::IndicesPtr output(new std::vector<int>); pcl::IndicesPtr output(new std::vector<int>);
@@ -316,7 +316,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
float max, float max,
bool negative) bool negative)
{ {
UASSERT(max > min); UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
@@ -336,7 +336,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
float max, float max,
bool negative) bool negative)
{ {
UASSERT(max > min); UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -356,7 +356,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
float max, float max,
bool negative) bool negative)
{ {
UASSERT(max > min); UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);