i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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) 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) if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
{ {
@@ -255,7 +255,7 @@ private:
obstaclesPub_.publish(rosCloud); 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: private:
src/nodelets/point_cloud_xyz.cpp
+5 -2
View File
@@ -64,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet
public: public:
PointCloudXYZ() : PointCloudXYZ() :
maxDepth_(0.0), maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0), voxelSize_(0.0),
decimation_(1), decimation_(1),
noiseFilterRadius_(0.0), noiseFilterRadius_(0.0),
@@ -100,6 +101,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); 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) 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) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -284,6 +286,7 @@ private:
private: private:
double maxDepth_; double maxDepth_;
double minDepth_;
double voxelSize_; double voxelSize_;
int decimation_; int decimation_;
double noiseFilterRadius_; double noiseFilterRadius_;
src/nodelets/point_cloud_xyzrgb.cpp
+5 -2
View File
@@ -64,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet
public: public:
PointCloudXYZRGB() : PointCloudXYZRGB() :
maxDepth_(0.0), maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0), voxelSize_(0.0),
decimation_(1), decimation_(1),
noiseFilterRadius_(0.0), noiseFilterRadius_(0.0),
@@ -97,6 +98,7 @@ private:
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); 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) 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) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
@@ -287,6 +289,7 @@ private:
private: private:
double maxDepth_; double maxDepth_;
double minDepth_;
double voxelSize_; double voxelSize_;
int decimation_; int decimation_;
double noiseFilterRadius_; double noiseFilterRadius_;