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

Nodelets point_cloud_xxxxxx: added min_depth parameter (fixed #66)

Browse Source
This commit is contained in:
Mathieu Labbe
2016-04-12 11:51:06 -04:00
parent f1636d3c7f
commit 04b87e1e15
3 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+2 -2
View File
@@ -104,7 +104,7 @@ private:
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
ros::Time time = ros::Time::now();
ros::WallTime time = ros::WallTime::now();
if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
{
@@ -255,7 +255,7 @@ private:
obstaclesPub_.publish(rosCloud);
}
//NODELET_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec());
//NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
}
private:
src/nodelets/point_cloud_xyz.cpp
+5 -2
View File
@@ -64,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet
public:
PointCloudXYZ() :
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
@@ -100,6 +101,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
@@ -254,9 +256,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
{
if(pclCloud->size() && maxDepth_ > 0)
if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -284,6 +286,7 @@ private:
private:
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
src/nodelets/point_cloud_xyzrgb.cpp
+5 -2
View File
@@ -64,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet
public:
PointCloudXYZRGB() :
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
@@ -97,6 +98,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
@@ -257,9 +259,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
{
if(pclCloud->size() && maxDepth_ > 0)
if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -287,6 +289,7 @@ private:
private:
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;