From c39f407b91742370113295d5193a42cf664a7615 Mon Sep 17 00:00:00 2001 From: bibarz Date: Fri, 12 Jun 2015 17:42:25 -0700 Subject: [PATCH 01/32] floor removal --- CMakeLists.txt | 1 + nodelet_plugins.xml | 9 +++ src/nodelets/obstacles_detection.cpp | 12 ++- src/nodelets/point_cloud_aggregator.cpp | 100 ++++++++++++++++++++++++ 4 files changed, 121 insertions(+), 1 deletion(-) create mode 100644 src/nodelets/point_cloud_aggregator.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 900bf734..f70639af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -133,6 +133,7 @@ SET(rtabmap_ros_lib_src src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp src/nodelets/obstacles_detection.cpp + src/nodelets/point_cloud_aggregator.cpp src/MsgConversion.cpp src/OdometryROS.cpp src/rviz/MapCloudDisplay.cpp diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 5d25ad92..d2aadf3a 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -54,4 +54,13 @@ This is my nodelet. + + + + This is my nodelet. + + + diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 1f47dae4..73ce1da3 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -70,6 +70,7 @@ public: normalEstimationRadius_(0.05), groundNormalAngle_(M_PI_4), minClusterSize_(20), + maxFloorHeight_(-1), maxObstaclesHeight_(0), waitForTransform_(false) {} @@ -90,6 +91,7 @@ private: pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -144,7 +146,7 @@ private: } pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + if((groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers()) && ground.get() && ground->size()) { pcl::copyPointCloud(*cloud, *ground, *groundCloud); } @@ -153,6 +155,13 @@ private: { pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); } + if(maxFloorHeight_ > 0 && (groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())) + { + pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); + flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); + *obstaclesCloud += *flatObstaclesCloud; + groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + } if(groundPub_.getNumSubscribers()) { @@ -184,6 +193,7 @@ private: double groundNormalAngle_; int minClusterSize_; double maxObstaclesHeight_; + double maxFloorHeight_; bool waitForTransform_; tf::TransformListener tfListener_; diff --git a/src/nodelets/point_cloud_aggregator.cpp b/src/nodelets/point_cloud_aggregator.cpp new file mode 100644 index 00000000..f83b561e --- /dev/null +++ b/src/nodelets/point_cloud_aggregator.cpp @@ -0,0 +1,100 @@ + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include + +#include + +namespace rtabmap_ros +{ + +class PointCloudAggregator : public nodelet::Nodelet +{ +public: + PointCloudAggregator() + {} + + virtual ~PointCloudAggregator() + {} + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + + cloudSub_1_ = nh.subscribe("cloud1", 1, &PointCloudAggregator::callback_1, this); + cloudSub_2_ = nh.subscribe("cloud2", 1, &PointCloudAggregator::callback_2, this); + cloudSub_3_ = nh.subscribe("cloud3", 1, &PointCloudAggregator::callback_3, this); + + cloudPub_ = nh.advertise("combined_cloud", 1); + } + + void callback_1(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::fromROSMsg(*cloudMsg, cloud1); + gather_and_publish(cloudMsg); + } + } + + void callback_2(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::fromROSMsg(*cloudMsg, cloud2); + gather_and_publish(cloudMsg); + } + } + + void callback_3(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::fromROSMsg(*cloudMsg, cloud3); + gather_and_publish(cloudMsg); + } + } + + void gather_and_publish(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + pcl::PointCloud totalCloud; + totalCloud = cloud1 + cloud2; + totalCloud += cloud3; + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(totalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = cloudMsg->header.frame_id; + cloudPub_.publish(rosCloud); + } + +private: + ros::Subscriber cloudSub_1_; + ros::Subscriber cloudSub_2_; + ros::Subscriber cloudSub_3_; + pcl::PointCloud cloud1, cloud2, cloud3; + + ros::Publisher cloudPub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet); +} + From c5be6bd934efed24dfbda55547d844d0f7dd84c5 Mon Sep 17 00:00:00 2001 From: bibarz Date: Mon, 15 Jun 2015 14:29:19 -0700 Subject: [PATCH 02/32] sync --- src/nodelets/point_cloud_aggregator.cpp | 86 +++++++++++-------------- 1 file changed, 37 insertions(+), 49 deletions(-) diff --git a/src/nodelets/point_cloud_aggregator.cpp b/src/nodelets/point_cloud_aggregator.cpp index f83b561e..a1fd1b1c 100644 --- a/src/nodelets/point_cloud_aggregator.cpp +++ b/src/nodelets/point_cloud_aggregator.cpp @@ -16,6 +16,7 @@ #include #include +#include #include @@ -25,71 +26,58 @@ namespace rtabmap_ros class PointCloudAggregator : public nodelet::Nodelet { public: - PointCloudAggregator() + PointCloudAggregator() : sync(NULL) {} virtual ~PointCloudAggregator() - {} + { + if (sync!=NULL) delete sync; + } private: + void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::fromROSMsg(*cloudMsg_1, cloud1); + pcl::fromROSMsg(*cloudMsg_2, cloud2); + pcl::fromROSMsg(*cloudMsg_3, cloud3); + pcl::PointCloud totalCloud; + totalCloud = cloud1 + cloud2; + totalCloud += cloud3; + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(totalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg_1->header.stamp; + rosCloud.header.frame_id = cloudMsg_1->header.frame_id; + cloudPub_.publish(rosCloud); + } + } + + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; virtual void onInit() { ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 5; pnh.param("queue_size", queueSize, queueSize); - cloudSub_1_ = nh.subscribe("cloud1", 1, &PointCloudAggregator::callback_1, this); - cloudSub_2_ = nh.subscribe("cloud2", 1, &PointCloudAggregator::callback_2, this); - cloudSub_3_ = nh.subscribe("cloud3", 1, &PointCloudAggregator::callback_3, this); + cloudSub_1_.subscribe(nh, "cloud1", 1); + cloudSub_2_.subscribe(nh, "cloud2", 1); + cloudSub_3_.subscribe(nh, "cloud3", 1); + + sync = new message_filters::Synchronizer(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3)); cloudPub_ = nh.advertise("combined_cloud", 1); } - void callback_1(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - if(cloudPub_.getNumSubscribers()) - { - pcl::fromROSMsg(*cloudMsg, cloud1); - gather_and_publish(cloudMsg); - } - } - - void callback_2(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - if(cloudPub_.getNumSubscribers()) - { - pcl::fromROSMsg(*cloudMsg, cloud2); - gather_and_publish(cloudMsg); - } - } - - void callback_3(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - if(cloudPub_.getNumSubscribers()) - { - pcl::fromROSMsg(*cloudMsg, cloud3); - gather_and_publish(cloudMsg); - } - } - - void gather_and_publish(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - pcl::PointCloud totalCloud; - totalCloud = cloud1 + cloud2; - totalCloud += cloud3; - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(totalCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = cloudMsg->header.frame_id; - cloudPub_.publish(rosCloud); - } - -private: - ros::Subscriber cloudSub_1_; - ros::Subscriber cloudSub_2_; - ros::Subscriber cloudSub_3_; + message_filters::Synchronizer* sync; + message_filters::Subscriber cloudSub_1_; + message_filters::Subscriber cloudSub_2_; + message_filters::Subscriber cloudSub_3_; pcl::PointCloud cloud1, cloud2, cloud3; ros::Publisher cloudPub_; From 0cbdb5cd1798950519f15acd27d19f22c0cf92ae Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Sat, 20 Jun 2015 19:57:19 -0700 Subject: [PATCH 03/32] timing --- src/nodelets/obstacles_detection.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 73ce1da3..0390dfac 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -98,6 +98,8 @@ private: groundPub_ = nh.advertise("ground", 1); obstaclesPub_ = nh.advertise("obstacles", 1); + + this->_lastFrameTime = ros::Time::now(); } @@ -106,6 +108,8 @@ private: { if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers()) { + + rtabmap::Transform localTransform; try { @@ -113,7 +117,7 @@ private: { if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) { - ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); return; } } @@ -123,10 +127,11 @@ private: } catch(tf::TransformException & ex) { - ROS_WARN("%s",ex.what()); + ROS_ERROR("%s",ex.what()); return; } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *cloud); pcl::IndicesPtr ground, obstacles; @@ -140,8 +145,17 @@ private: } if(cloud->size()) { + ros::Time lasttime = ros::Time::now(); rtabmap::util3d::segmentObstaclesFromGround(cloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + + ros::Time curtime = ros::Time::now(); + ros::Duration process_duration = curtime - lasttime; + ros::Duration between_frames = curtime - this->_lastFrameTime; + this->_lastFrameTime = curtime; + std::stringstream buffer; + buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; + ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); } } @@ -184,6 +198,7 @@ private: //publish the message obstaclesPub_.publish(rosCloud); } + } } @@ -202,6 +217,7 @@ private: ros::Publisher obstaclesPub_; ros::Subscriber cloudSub_; + ros::Time _lastFrameTime; }; PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet); From a43def5dbe44f4a196e1af1ea5d38687534061c2 Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Mon, 22 Jun 2015 14:24:20 -0700 Subject: [PATCH 04/32] refactoring --- src/nodelets/obstacles_detection.cpp | 194 ++++++++++++++------------- 1 file changed, 102 insertions(+), 92 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 0390dfac..9992fe1c 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -106,100 +106,110 @@ private: void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { - if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers()) + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0) { - - - rtabmap::Transform localTransform; - try - { - if(waitForTransform_) - { - if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) - { - ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); - return; - } - } - tf::StampedTransform tmp; - tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); - localTransform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - ROS_ERROR("%s",ex.what()); - return; - } - - - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *cloud); - pcl::IndicesPtr ground, obstacles; - if(cloud->size()) - { - cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform); - - if(maxObstaclesHeight_ > 0) - { - cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); - } - if(cloud->size()) - { - ros::Time lasttime = ros::Time::now(); - rtabmap::util3d::segmentObstaclesFromGround(cloud, - ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - - ros::Time curtime = ros::Time::now(); - ros::Duration process_duration = curtime - lasttime; - ros::Duration between_frames = curtime - this->_lastFrameTime; - this->_lastFrameTime = curtime; - std::stringstream buffer; - buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; - ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); - } - } - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - if((groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers()) && ground.get() && ground->size()) - { - pcl::copyPointCloud(*cloud, *ground, *groundCloud); - } - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) - { - pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); - } - if(maxFloorHeight_ > 0 && (groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())) - { - pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); - flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); - *obstaclesCloud += *flatObstaclesCloud; - groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - } - - if(groundPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - groundPub_.publish(rosCloud); - } - - if(obstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - obstaclesPub_.publish(rosCloud); - } - + // no one wants the results + return; } + + rtabmap::Transform localTransform; + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + { + ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + localTransform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + ROS_ERROR("%s",ex.what()); + return; + } + + + pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *originalCloud); + if(originalCloud->size() == 0) + { + ROS_ERROR("Recieved empty point cloud!"); + return; + } + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + + ///////////////////////////////////////////////////////////////////////////// + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud, *cloud); + + if(maxObstaclesHeight_ > 0) + { + cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); + } + + ros::Time lasttime = ros::Time::now(); + + pcl::IndicesPtr ground, obstacles; + rtabmap::util3d::segmentObstaclesFromGround(cloud, + ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + + ros::Time curtime = ros::Time::now(); + ros::Duration process_duration = curtime - lasttime; + ros::Duration between_frames = curtime - this->_lastFrameTime; + this->_lastFrameTime = curtime; + std::stringstream buffer; + buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; + ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); + + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + if(ground.get() && ground->size()) + { + pcl::copyPointCloud(*cloud, *ground, *groundCloud); + } + + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); + } + + if(maxFloorHeight_ > 0) + { + pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); + flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); + *obstaclesCloud += *flatObstaclesCloud; + + groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + } + + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*groundCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + obstaclesPub_.publish(rosCloud); + } + } private: From 08f65901aa67572058b1edc23a562a2050dac60c Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Mon, 22 Jun 2015 14:48:48 -0700 Subject: [PATCH 05/32] print sizes --- src/nodelets/obstacles_detection.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 9992fe1c..bbb85558 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -163,9 +163,7 @@ private: ros::Duration process_duration = curtime - lasttime; ros::Duration between_frames = curtime - this->_lastFrameTime; this->_lastFrameTime = curtime; - std::stringstream buffer; - buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; - ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); if(ground.get() && ground->size()) @@ -179,14 +177,19 @@ private: pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); } - if(maxFloorHeight_ > 0) - { - pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); - flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); - *obstaclesCloud += *flatObstaclesCloud; + std::stringstream buffer; + buffer << "cloud=" << cloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); + buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; + ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); - groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - } +// if(maxFloorHeight_ > 0) +// { +// pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); +// flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); +// *obstaclesCloud += *flatObstaclesCloud; +// +// groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); +// } if(groundPub_.getNumSubscribers()) { From 33612c7b12b28a9dd72061499e8a23b742944aaa Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Mon, 22 Jun 2015 15:28:04 -0700 Subject: [PATCH 06/32] cut then extract --- src/nodelets/obstacles_detection.cpp | 58 ++++++++++++---------------- 1 file changed, 24 insertions(+), 34 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index bbb85558..e9e5bf10 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -71,7 +71,7 @@ public: groundNormalAngle_(M_PI_4), minClusterSize_(20), maxFloorHeight_(-1), - maxObstaclesHeight_(0), + //maxObstaclesHeight_(0), waitForTransform_(false) {} @@ -90,7 +90,7 @@ private: pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_); pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); - pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + //pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); @@ -145,52 +145,42 @@ private: ///////////////////////////////////////////////////////////////////////////// - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::copyPointCloud(*originalCloud, *cloud); - - if(maxObstaclesHeight_ > 0) - { - cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); - } + pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); ros::Time lasttime = ros::Time::now(); pcl::IndicesPtr ground, obstacles; - rtabmap::util3d::segmentObstaclesFromGround(cloud, + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + if(ground.get() && ground->size()) + { + pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); + } + + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, std::numeric_limits::max()); + + if(obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); + *obstaclesCloud += *obstaclesNearFloorCloud; + } + ros::Time curtime = ros::Time::now(); ros::Duration process_duration = curtime - lasttime; ros::Duration between_frames = curtime - this->_lastFrameTime; this->_lastFrameTime = curtime; - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - if(ground.get() && ground->size()) - { - pcl::copyPointCloud(*cloud, *ground, *groundCloud); - } - - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) - { - pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); - } - std::stringstream buffer; - buffer << "cloud=" << cloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); + buffer << "cloud=" << originalCloud->size() << " hypothetical ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); -// if(maxFloorHeight_ > 0) -// { -// pcl::PointCloud::Ptr flatObstaclesCloud(new pcl::PointCloud); -// flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits::max()); -// *obstaclesCloud += *flatObstaclesCloud; -// -// groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits::min(), maxFloorHeight_); -// } - if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; @@ -220,7 +210,7 @@ private: double normalEstimationRadius_; double groundNormalAngle_; int minClusterSize_; - double maxObstaclesHeight_; + //double maxObstaclesHeight_; double maxFloorHeight_; bool waitForTransform_; From 3cd48d670ad19a1eabdb51537cc8575945004432 Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Mon, 22 Jun 2015 15:49:13 -0700 Subject: [PATCH 07/32] tming of the whole function --- src/nodelets/obstacles_detection.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index e9e5bf10..69d7b411 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -155,6 +155,7 @@ private: ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); if(ground.get() && ground->size()) { @@ -171,16 +172,6 @@ private: *obstaclesCloud += *obstaclesNearFloorCloud; } - ros::Time curtime = ros::Time::now(); - ros::Duration process_duration = curtime - lasttime; - ros::Duration between_frames = curtime - this->_lastFrameTime; - this->_lastFrameTime = curtime; - - std::stringstream buffer; - buffer << "cloud=" << originalCloud->size() << " hypothetical ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); - buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; - ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); - if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; @@ -203,6 +194,16 @@ private: obstaclesPub_.publish(rosCloud); } + ros::Time curtime = ros::Time::now(); + + ros::Duration process_duration = curtime - lasttime; + ros::Duration between_frames = curtime - this->_lastFrameTime; + this->_lastFrameTime = curtime; + std::stringstream buffer; + buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); + buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; + ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); + } private: From 36442e4ece3e82a7e53fb5c52e2a028ac2943495 Mon Sep 17 00:00:00 2001 From: Oleg Sinyavskiy Date: Mon, 22 Jun 2015 15:54:54 -0700 Subject: [PATCH 08/32] add obst height back --- src/nodelets/obstacles_detection.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 69d7b411..9efb0683 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -71,7 +71,7 @@ public: groundNormalAngle_(M_PI_4), minClusterSize_(20), maxFloorHeight_(-1), - //maxObstaclesHeight_(0), + maxObstaclesHeight_(1.5), waitForTransform_(false) {} @@ -90,7 +90,7 @@ private: pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_); pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); - //pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); @@ -163,7 +163,9 @@ private: } pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, std::numeric_limits::max()); + + + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); if(obstacles.get() && obstacles->size()) { @@ -211,7 +213,7 @@ private: double normalEstimationRadius_; double groundNormalAngle_; int minClusterSize_; - //double maxObstaclesHeight_; + double maxObstaclesHeight_; double maxFloorHeight_; bool waitForTransform_; From 0c1e7ec153468bc88e9a12169941712e78821877 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 16:48:12 -0700 Subject: [PATCH 09/32] Add option for simple extration --- src/nodelets/obstacles_detection.cpp | 45 +++++++++++++++------------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 9efb0683..f22b3f00 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -72,7 +72,8 @@ public: minClusterSize_(20), maxFloorHeight_(-1), maxObstaclesHeight_(1.5), - waitForTransform_(false) + waitForTransform_(false), + simpleSegmentation_(false) {} virtual ~ObstaclesDetection() @@ -93,6 +94,7 @@ private: pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); + pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -148,31 +150,33 @@ private: pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + ros::Time lasttime = ros::Time::now(); pcl::IndicesPtr ground, obstacles; - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, - ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - if(ground.get() && ground->size()) - { - pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); + + if (!simpleSegmentation_){ + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, + ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + + if(ground.get() && ground->size()) + { + pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); + } + if(obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); + *obstaclesCloud += *obstaclesNearFloorCloud; + } + } + else{ + groundCloud = hypotheticalGroundCloud; } - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - - - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - - if(obstacles.get() && obstacles->size()) - { - pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); - *obstaclesCloud += *obstaclesNearFloorCloud; - } if(groundPub_.getNumSubscribers()) { @@ -216,6 +220,7 @@ private: double maxObstaclesHeight_; double maxFloorHeight_; bool waitForTransform_; + bool simpleSegmentation_; tf::TransformListener tfListener_; From 568ce4ebc1c2bec6a6e2fe16ef617b27c40950df Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 16:49:54 -0700 Subject: [PATCH 10/32] Commented line for testing --- src/nodelets/obstacles_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index f22b3f00..1619015d 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -208,7 +208,7 @@ private: std::stringstream buffer; buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; - ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); + //ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); } From 5e4ededf7a2827170f55ede10781d3a9116eeda8 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:19:55 -0700 Subject: [PATCH 11/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 1619015d..9cc9c541 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -114,6 +114,9 @@ private: return; } + ROS_ERROR("1111111111111111111111"); + + rtabmap::Transform localTransform; try { @@ -125,6 +128,9 @@ private: return; } } + + ROS_ERROR("2222222222222222222222222222222"); + tf::StampedTransform tmp; tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); localTransform = rtabmap_ros::transformFromTF(tmp); @@ -145,6 +151,8 @@ private: } originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + ROS_ERROR("3333333333333333333333333"); + ///////////////////////////////////////////////////////////////////////////// pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); @@ -159,6 +167,8 @@ private: pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); if (!simpleSegmentation_){ + ROS_ERROR("44444444444444444444444444444"); + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); @@ -174,6 +184,7 @@ private: } } else{ + ROS_ERROR("555555555555555555555555"); groundCloud = hypotheticalGroundCloud; } From b10ee02eb4e68b5bbef480101150ec0056fc8b05 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:36:11 -0700 Subject: [PATCH 12/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 9cc9c541..aeaee536 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -114,9 +114,6 @@ private: return; } - ROS_ERROR("1111111111111111111111"); - - rtabmap::Transform localTransform; try { @@ -158,9 +155,13 @@ private: pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + ROS_ERROR("AAAa3333333333333333333333333"); + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + ROS_ERROR("BBBb3333333333333333333333333"); + ros::Time lasttime = ros::Time::now(); pcl::IndicesPtr ground, obstacles; From 6c1fd0e7284d33b2c66c2dc1eae0491703475483 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:44:34 -0700 Subject: [PATCH 13/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index aeaee536..71287060 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -158,7 +158,6 @@ private: ROS_ERROR("AAAa3333333333333333333333333"); pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); ROS_ERROR("BBBb3333333333333333333333333"); @@ -177,6 +176,9 @@ private: { pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); } + + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); @@ -186,6 +188,7 @@ private: } else{ ROS_ERROR("555555555555555555555555"); + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); groundCloud = hypotheticalGroundCloud; } From 0b0406723fb86f859e89387c3fa2e5ec48a400fc Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:47:20 -0700 Subject: [PATCH 14/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 71287060..901b1c19 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -179,17 +179,24 @@ private: obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + ROS_ERROR("RRR 44444444444444444444444444444"); + if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); *obstaclesCloud += *obstaclesNearFloorCloud; } + ROS_ERROR("R 44444444444444444444444444444"); + } else{ ROS_ERROR("555555555555555555555555"); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + ROS_ERROR("RRR 555555555555555555555555"); + groundCloud = hypotheticalGroundCloud; + } From 04beb41c488eb7d25a48b0416c295e85b35bab59 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:53:14 -0700 Subject: [PATCH 15/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 30 +++++++++++----------------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 901b1c19..b2fcd5dd 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -126,7 +126,6 @@ private: } } - ROS_ERROR("2222222222222222222222222222222"); tf::StampedTransform tmp; tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); @@ -148,38 +147,38 @@ private: } originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - ROS_ERROR("3333333333333333333333333"); ///////////////////////////////////////////////////////////////////////////// pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - - ROS_ERROR("AAAa3333333333333333333333333"); - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - - ROS_ERROR("BBBb3333333333333333333333333"); - - ros::Time lasttime = ros::Time::now(); - pcl::IndicesPtr ground, obstacles; pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + ros::Time lasttime = ros::Time::now(); + if (!simpleSegmentation_){ - ROS_ERROR("44444444444444444444444444444"); + + ROS_ERROR("1-1"); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + + ROS_ERROR("1-2"); rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + ROS_ERROR("1-3"); + if(ground.get() && ground->size()) { pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); } + ROS_ERROR("1-4"); + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - ROS_ERROR("RRR 44444444444444444444444444444"); + ROS_ERROR("1-5"); if(obstacles.get() && obstacles->size()) { @@ -191,12 +190,7 @@ private: } else{ - ROS_ERROR("555555555555555555555555"); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - ROS_ERROR("RRR 555555555555555555555555"); - - groundCloud = hypotheticalGroundCloud; - } From d4bd5adc9afbe1b3cedaa775b167cd9da727bb00 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 17:57:22 -0700 Subject: [PATCH 16/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index b2fcd5dd..5ffae9d0 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -161,7 +161,6 @@ private: ROS_ERROR("1-1"); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - ROS_ERROR("1-2"); rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, @@ -190,7 +189,9 @@ private: } else{ + ROS_ERROR("2-1"); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + ROS_ERROR("2-2"); } From 89123f57bf1bd68e7bdf28a26b8a487b75726c66 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Tue, 7 Jul 2015 18:02:15 -0700 Subject: [PATCH 17/32] Comment for debugging --- src/nodelets/obstacles_detection.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 5ffae9d0..79ee6bb2 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -195,6 +195,7 @@ private: } + /* if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; @@ -204,7 +205,7 @@ private: //publish the message groundPub_.publish(rosCloud); - } + }*/ if(obstaclesPub_.getNumSubscribers()) { From cc139c6664978d2032d275804e058837b0a29185 Mon Sep 17 00:00:00 2001 From: JRombouts Date: Wed, 8 Jul 2015 12:38:35 -0700 Subject: [PATCH 18/32] Adding an option to cut incoming depth --- src/nodelets/obstacles_detection.cpp | 27 +++++++++------------------ src/nodelets/point_cloud_xyz.cpp | 20 +++++++++++++++++--- 2 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 79ee6bb2..d262d54d 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -157,55 +157,46 @@ private: ros::Time lasttime = ros::Time::now(); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + if (!simpleSegmentation_){ - ROS_ERROR("1-1"); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - ROS_ERROR("1-2"); rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - ROS_ERROR("1-3"); - if(ground.get() && ground->size()) { pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); } - ROS_ERROR("1-4"); - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - ROS_ERROR("1-5"); - if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); *obstaclesCloud += *obstaclesNearFloorCloud; } - ROS_ERROR("R 44444444444444444444444444444"); } else{ - ROS_ERROR("2-1"); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - ROS_ERROR("2-2"); } - /* + if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*groundCloud, rosCloud); + if (simpleSegmentation_) {pcl::toROSMsg(*hypotheticalGroundCloud, rosCloud);;} + else {pcl::toROSMsg(*groundCloud, rosCloud);} rosCloud.header.stamp = cloudMsg->header.stamp; rosCloud.header.frame_id = frameId_; //publish the message groundPub_.publish(rosCloud); - }*/ + } if(obstaclesPub_.getNumSubscribers()) { @@ -223,9 +214,9 @@ private: ros::Duration process_duration = curtime - lasttime; ros::Duration between_frames = curtime - this->_lastFrameTime; this->_lastFrameTime = curtime; - std::stringstream buffer; - buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); - buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; + //std::stringstream buffer; + //buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); + //buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; //ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); } diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index e944b9cf..3207371f 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -69,7 +69,8 @@ public: approxSyncDepth_(0), approxSyncDisparity_(0), exactSyncDepth_(0), - exactSyncDisparity_(0) + exactSyncDisparity_(0), + cut_(0) {} virtual ~PointCloudXYZ() @@ -99,6 +100,7 @@ private: pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); + pnh.param("cut", cut_, cut_); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) @@ -147,6 +149,18 @@ private: if(cloudPub_.getNumSubscribers()) { cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv::Mat image=imageDepthPtr->image; + int rows = image.rows; + int cols = image.cols; + + if (cut_>0){ + cv::Mat pRoi = image(cv::Rect(0, 0, cut_, rows)); + pRoi.setTo(cv::Scalar(0.)); + } + else if (cut_<0){ + cv::Mat pRoi = image(cv::Rect(cols+cut_, 0, -cut_, rows)); + pRoi.setTo(cv::Scalar(0.)); + } image_geometry::PinholeCameraModel model; model.fromCameraInfo(*cameraInfo); @@ -157,13 +171,12 @@ private: pcl::PointCloud::Ptr pclCloud; pclCloud = rtabmap::util3d::cloudFromDepth( - imageDepthPtr->image, + image, cx, cy, fx, fy, decimation_); - processAndPublish(pclCloud, depth->header); } } @@ -245,6 +258,7 @@ private: int decimation_; double noiseFilterRadius_; int noiseFilterMinNeighbors_; + int cut_; ros::Publisher cloudPub_; From 37a53d91c299d3d122dc282ac1383f0120a87801 Mon Sep 17 00:00:00 2001 From: JRombouts Date: Wed, 8 Jul 2015 12:49:15 -0700 Subject: [PATCH 19/32] Adding an option to cut incoming depth --- src/nodelets/point_cloud_xyz.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 3207371f..d655231b 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -70,7 +70,8 @@ public: approxSyncDisparity_(0), exactSyncDepth_(0), exactSyncDisparity_(0), - cut_(0) + cut_right_(0), + cut_left_(0) {} virtual ~PointCloudXYZ() @@ -100,7 +101,8 @@ private: pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - pnh.param("cut", cut_, cut_); + pnh.param("cut_left", cut_left_, cut_left_); + pnh.param("cut_right", cut_right_, cut_right_); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) @@ -153,12 +155,12 @@ private: int rows = image.rows; int cols = image.cols; - if (cut_>0){ - cv::Mat pRoi = image(cv::Rect(0, 0, cut_, rows)); + if (cut_left_>0){ + cv::Mat pRoi = image(cv::Rect(0, 0, cut_left_, rows)); pRoi.setTo(cv::Scalar(0.)); } - else if (cut_<0){ - cv::Mat pRoi = image(cv::Rect(cols+cut_, 0, -cut_, rows)); + if (cut_right_<0){ + cv::Mat pRoi = image(cv::Rect(cols-cut_right_, 0, cut_right_, rows)); pRoi.setTo(cv::Scalar(0.)); } @@ -258,7 +260,8 @@ private: int decimation_; double noiseFilterRadius_; int noiseFilterMinNeighbors_; - int cut_; + int cut_left_; + int cut_right_; ros::Publisher cloudPub_; From 59986a80668c8abefe7856d40d886e47530df8b7 Mon Sep 17 00:00:00 2001 From: JRombouts Date: Wed, 8 Jul 2015 21:36:06 -0700 Subject: [PATCH 20/32] Publish even when pointcloud is null, otherwise system hangs --- src/nodelets/obstacles_detection.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index d262d54d..70056b88 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -140,11 +140,35 @@ private: pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *originalCloud); + if(originalCloud->size() == 0) { ROS_ERROR("Recieved empty point cloud!"); + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*originalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*originalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + obstaclesPub_.publish(rosCloud); + } return; } + + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); From e9d27481f3a21e69f7adfd18895e37bc9e73aff1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 9 Jul 2015 12:26:20 -0400 Subject: [PATCH 21/32] Update README.md --- README.md | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 80ad6168..ced87365 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,10 @@ For the RTAB-Map libraries and standalone application, visit the [RTAB-Map's hom ### ROS distribution RTAB-Map is released as binaries in the ROS distribution. + * Jade + ``` +$ sudo apt-get install ros-jade-rtabmap-ros +``` * Indigo ``` $ sudo apt-get install ros-indigo-rtabmap-ros @@ -21,13 +25,13 @@ $ sudo apt-get install ros-hydro-rtabmap-ros ``` ### Build from source -This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo (**Fuerte and Groovy are not supported**). - * **Note for ROS Indigo**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. +This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo/Jade (**Fuerte and Groovy are not supported**). + * **Note for ROS Indigo/Jade**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. * The next instructions assume that you have setup your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). The workspace path is `~/catkin_ws` and your `~/.bashrc` contains: ```bash -source /opt/ros/hydro/setup.bash +source /opt/ros/[hydro|indigo|jade]/setup.bash source ~/catkin_ws/devel/setup.bash ``` From f75744217cb3d2e5eaffc4a6f10add07f043bc53 Mon Sep 17 00:00:00 2001 From: JRombouts Date: Fri, 10 Jul 2015 17:01:08 -0700 Subject: [PATCH 22/32] New option for detecting close objects and other hacks for closer objects --- src/nodelets/obstacles_detection.cpp | 56 +++++++++++++++++++++++++++- src/nodelets/point_cloud_xyz.cpp | 29 +++++++++++++- 2 files changed, 82 insertions(+), 3 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 70056b88..a75b74ba 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -169,7 +169,7 @@ private: } - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + ///////////////////////////////////////////////////////////////////////////// @@ -181,10 +181,12 @@ private: ros::Time lasttime = ros::Time::now(); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); +/* if (!simpleSegmentation_){ + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); @@ -203,8 +205,58 @@ private: *obstaclesCloud += *obstaclesNearFloorCloud; } + }*/ + if (!simpleSegmentation_){ + + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + pcl::PointCloud::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + + pcl::PointCloud::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(originalCloud_front, "z", std::numeric_limits::min(), maxFloorHeight_); + pcl::PointCloud::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(originalCloud_back, "z", std::numeric_limits::min(), maxFloorHeight_); + + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + + //STEP 1. + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_front, + ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); + + if(ground.get() && ground->size()) + { + pcl::copyPointCloud(*hypotheticalGroundCloud_front, *ground, *groundCloud); + } + + + if(obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_front, *obstacles, *obstaclesNearFloorCloud); + *obstaclesCloud += *obstaclesNearFloorCloud; + } + + //STEP 2. + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_back, + ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_); + + if(ground.get() && ground->size()) + { + pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_back, *ground, *groundCloud2); + *groundCloud += *groundCloud2; + } + + + if(obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorCloud); + *obstaclesCloud += *obstaclesNearFloorCloud; + } + } else{ + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); } diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index d655231b..540296b9 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -71,7 +71,8 @@ public: exactSyncDepth_(0), exactSyncDisparity_(0), cut_right_(0), - cut_left_(0) + cut_left_(0), + special_filter_close_object_(false) {} virtual ~PointCloudXYZ() @@ -103,6 +104,8 @@ private: pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); pnh.param("cut_left", cut_left_, cut_left_); pnh.param("cut_right", cut_right_, cut_right_); + pnh.param("special_filter_close_object", special_filter_close_object_, special_filter_close_object_); + ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) @@ -164,6 +167,29 @@ private: pRoi.setTo(cv::Scalar(0.)); } + if (special_filter_close_object_){ + cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); + //cv::GaussianBlur(pRoi, pRoi, cv::Size(3, 3), 0, 0); + cv::medianBlur(pRoi, pRoi, 3); + + //Do filter of close objects + pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows)))); + cv::Mat bluredImage=pRoi.clone(); + + //Working Ok with 15 / 15 + cv::GaussianBlur(pRoi, bluredImage, cv::Size(5, 5), 0, 0); + + for(int y = 0; y < bluredImage.cols; y++) + for(int x = 0; x < bluredImage.rows; x++){ + if (bluredImage.at(x,y) == 0){ + pRoi.at(x,y) = 400; + } + } + } + + + + image_geometry::PinholeCameraModel model; model.fromCameraInfo(*cameraInfo); float fx = model.fx(); @@ -262,6 +288,7 @@ private: int noiseFilterMinNeighbors_; int cut_left_; int cut_right_; + bool special_filter_close_object_; ros::Publisher cloudPub_; From b98cff41e3bfda8253724a5b406902829d295312 Mon Sep 17 00:00:00 2001 From: JRombouts Date: Mon, 13 Jul 2015 13:27:37 -0700 Subject: [PATCH 23/32] Only segement obstacle that are at least 0.8m away, this is a hack to remove noisy reading AND the fact that we don't clear the costmap when something is closer than 75cm from the robot --- src/nodelets/obstacles_detection.cpp | 1 + src/nodelets/point_cloud_xyz.cpp | 5 ----- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index a75b74ba..9a6bf1e0 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -216,6 +216,7 @@ private: pcl::PointCloud::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(originalCloud_back, "z", std::numeric_limits::min(), maxFloorHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits::max()); //STEP 1. rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_front, diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 540296b9..ad423940 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -169,7 +169,6 @@ private: if (special_filter_close_object_){ cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); - //cv::GaussianBlur(pRoi, pRoi, cv::Size(3, 3), 0, 0); cv::medianBlur(pRoi, pRoi, 3); //Do filter of close objects @@ -186,10 +185,6 @@ private: } } } - - - - image_geometry::PinholeCameraModel model; model.fromCameraInfo(*cameraInfo); float fx = model.fx(); From cd7e7bd37fab7f38a6c308a9edb15ebbcb3c161a Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Mon, 13 Jul 2015 13:28:50 -0700 Subject: [PATCH 24/32] Adding documentation --- src/nodelets/obstacles_detection.cpp | 21 +++++++++++++++------ src/nodelets/point_cloud_xyz.cpp | 8 ++------ 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index a75b74ba..53378cd5 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -73,7 +73,8 @@ public: maxFloorHeight_(-1), maxObstaclesHeight_(1.5), waitForTransform_(false), - simpleSegmentation_(false) + simpleSegmentation_(false), + optimizeForCloseObject_(true) {} virtual ~ObstaclesDetection() @@ -95,6 +96,7 @@ private: pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_); + pnh.param("optimize_for_close_object", optimizeForCloseObject_, optimizeForCloseObject_); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -181,9 +183,7 @@ private: ros::Time lasttime = ros::Time::now(); - -/* - if (!simpleSegmentation_){ + if (!simpleSegmentation_ && !optimizeForCloseObject_){ originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); @@ -205,8 +205,16 @@ private: *obstaclesCloud += *obstaclesNearFloorCloud; } - }*/ - if (!simpleSegmentation_){ + } + if (!simpleSegmentation_ && optimizeForCloseObject_){ + // If the option optimize for close object has been set to true, + // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m + // one for potential floor points further away than 1m. + // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, + // which allows to detect smaller objects, without increasing the number of false positive. + // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the + // grond normal angle (* 2.). + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); pcl::PointCloud::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); @@ -306,6 +314,7 @@ private: double maxFloorHeight_; bool waitForTransform_; bool simpleSegmentation_; + bool optimizeForCloseObject_; tf::TransformListener tfListener_; diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 540296b9..f4da601d 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -168,9 +168,8 @@ private: } if (special_filter_close_object_){ - cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); - //cv::GaussianBlur(pRoi, pRoi, cv::Size(3, 3), 0, 0); - cv::medianBlur(pRoi, pRoi, 3); + //cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); + //cv::medianBlur(pRoi, pRoi, 3); //Do filter of close objects pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows)))); @@ -187,9 +186,6 @@ private: } } - - - image_geometry::PinholeCameraModel model; model.fromCameraInfo(*cameraInfo); float fx = model.fx(); From bd7961670337ce96673f1d58f29c287b3ca95d89 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Mon, 13 Jul 2015 14:57:41 -0700 Subject: [PATCH 25/32] Add comments --- src/nodelets/obstacles_detection.cpp | 5 ----- src/nodelets/point_cloud_xyz.cpp | 14 +++++++++++--- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 9a6bf1e0..08837449 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -291,11 +291,6 @@ private: ros::Duration process_duration = curtime - lasttime; ros::Duration between_frames = curtime - this->_lastFrameTime; this->_lastFrameTime = curtime; - //std::stringstream buffer; - //buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size(); - //buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz"; - //ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str()); - } private: diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index ad423940..d15bc2ce 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -158,6 +158,10 @@ private: int rows = image.rows; int cols = image.cols; + //Cut left and cut right options to mask the image. + //If cut_left (resp. cut_right) is set to a positive value, we set the first (resp. last) columns + //of the depth image to 0, meaning that no depth reading has been received. + //Number of columns to be masked is equal to cut_left (resp. cut_right value) if (cut_left_>0){ cv::Mat pRoi = image(cv::Rect(0, 0, cut_left_, rows)); pRoi.setTo(cv::Scalar(0.)); @@ -167,6 +171,13 @@ private: pRoi.setTo(cv::Scalar(0.)); } + //This option enables a filter for close object. + //Fist, we do a median blur on the image to get rid of potential noise + //Second, we set all false reading that are likely due to an object sitting in front of the camera + // to a short distance estimation (here, 40cm). + //This hence make the assumption that the depth camera is looking forward and sees the floor on + // the bottom rows of the depth image + //This option is highly experimental and should be used with extreme care. if (special_filter_close_object_){ cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); cv::medianBlur(pRoi, pRoi, 3); @@ -174,10 +185,7 @@ private: //Do filter of close objects pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows)))); cv::Mat bluredImage=pRoi.clone(); - - //Working Ok with 15 / 15 cv::GaussianBlur(pRoi, bluredImage, cv::Size(5, 5), 0, 0); - for(int y = 0; y < bluredImage.cols; y++) for(int x = 0; x < bluredImage.rows; x++){ if (bluredImage.at(x,y) == 0){ From ce219a9a34b99c72ea5d187faae6cb9f5b61d8ad Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Mon, 13 Jul 2015 15:04:53 -0700 Subject: [PATCH 26/32] Adding comments --- src/nodelets/obstacles_detection.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 34dc44c2..9a0bd290 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -127,8 +127,6 @@ private: return; } } - - tf::StampedTransform tmp; tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); localTransform = rtabmap_ros::transformFromTF(tmp); @@ -139,10 +137,11 @@ private: return; } - pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *originalCloud); + //Even if the original cloud is empty, we need to publish the empty cloud, + //Otherwise, the aggregator of point cloud would wait indefinitely to get a valid pointcloud if(originalCloud->size() == 0) { ROS_ERROR("Recieved empty point cloud!"); @@ -170,12 +169,7 @@ private: return; } - - - - - ///////////////////////////////////////////////////////////////////////////// - + //Common variables for all strategies pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); pcl::IndicesPtr ground, obstacles; @@ -184,7 +178,11 @@ private: ros::Time lasttime = ros::Time::now(); if (!simpleSegmentation_ && !optimizeForCloseObject_){ - + //This is the default strategy + //The cloud is divided into two pointcloud based on reported Z and the position of the camera. + // One is the hypothetical ground cloud and the other one is the obstacles pointcloud. + //The algorithm then extracts (and remove) from the hypothetical ground cloud the detected obstacles, + //and add them to the obstacles pointcloud originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); @@ -215,7 +213,6 @@ private: // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the // grond normal angle (* 2.). - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); pcl::PointCloud::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); pcl::PointCloud::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); @@ -264,13 +261,14 @@ private: } else{ + //If the option simple segmentation has been set to true, + //the floor is always the hypothetical ground cloud, with is a cut-off based on the z estimation of each point originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); } - if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; From ef7c63ca2048b2d3904b9b67acd3d5b134781b8b Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Mon, 13 Jul 2015 17:18:13 -0700 Subject: [PATCH 27/32] Addressing pull request comments --- src/nodelets/obstacles_detection.cpp | 33 ++++++++++------------------ src/nodelets/point_cloud_xyz.cpp | 20 +++++++++-------- 2 files changed, 22 insertions(+), 31 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 9a0bd290..02b68e94 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -177,14 +177,16 @@ private: ros::Time lasttime = ros::Time::now(); + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); + obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); + if (!simpleSegmentation_ && !optimizeForCloseObject_){ //This is the default strategy //The cloud is divided into two pointcloud based on reported Z and the position of the camera. // One is the hypothetical ground cloud and the other one is the obstacles pointcloud. //The algorithm then extracts (and remove) from the hypothetical ground cloud the detected obstacles, //and add them to the obstacles pointcloud - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); @@ -194,8 +196,6 @@ private: pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); } - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); @@ -213,14 +213,9 @@ private: // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the // grond normal angle (* 2.). - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - pcl::PointCloud::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); - pcl::PointCloud::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + pcl::PointCloud::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits::max()); - pcl::PointCloud::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(originalCloud_front, "z", std::numeric_limits::min(), maxFloorHeight_); - pcl::PointCloud::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(originalCloud_back, "z", std::numeric_limits::min(), maxFloorHeight_); - - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits::max()); //STEP 1. @@ -254,20 +249,14 @@ private: if(obstacles.get() && obstacles->size()) { - pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorCloud); - *obstaclesCloud += *obstaclesNearFloorCloud; + pcl::PointCloud::Ptr obstaclesNearFloorBackCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorBackCloud); + *obstaclesCloud += *obstaclesNearFloorBackCloud; } } - else{ - //If the option simple segmentation has been set to true, - //the floor is always the hypothetical ground cloud, with is a cut-off based on the z estimation of each point - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - } - + //If the option simple segmentation has been set to true, + //the floor is always the hypothetical ground cloud, with is a cut-off based on the z estimation of each point if(groundPub_.getNumSubscribers()) { diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 30e36152..a72cd55e 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -72,7 +72,7 @@ public: exactSyncDisparity_(0), cut_right_(0), cut_left_(0), - special_filter_close_object_(false) + create_close_obstacle_if_depth_is_missing_(false) {} virtual ~PointCloudXYZ() @@ -104,7 +104,7 @@ private: pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); pnh.param("cut_left", cut_left_, cut_left_); pnh.param("cut_right", cut_right_, cut_right_); - pnh.param("special_filter_close_object", special_filter_close_object_, special_filter_close_object_); + pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); @@ -178,17 +178,19 @@ private: //This hence make the assumption that the depth camera is looking forward and sees the floor on // the bottom rows of the depth image //This option is highly experimental and should be used with extreme care. - if (special_filter_close_object_){ + if (create_close_obstacle_if_depth_is_missing_){ cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); cv::medianBlur(pRoi, pRoi, 3); //Do filter of close objects + //If the depth is registered, there is usually a black frame around the depth image + //Hence, the ROI stops before the expected "frame" pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows)))); - cv::Mat bluredImage=pRoi.clone(); - cv::GaussianBlur(pRoi, bluredImage, cv::Size(5, 5), 0, 0); - for(int y = 0; y < bluredImage.cols; y++) - for(int x = 0; x < bluredImage.rows; x++){ - if (bluredImage.at(x,y) == 0){ + cv::Mat blurredImage=pRoi.clone(); + cv::GaussianBlur(pRoi, blurredImage, cv::Size(5, 5), 0, 0); + for(int y = 0; y < blurredImage.cols; y++) + for(int x = 0; x < blurredImage.rows; x++){ + if (blurredImage.at(x,y) == 0){ pRoi.at(x,y) = 400; } } @@ -292,7 +294,7 @@ private: int noiseFilterMinNeighbors_; int cut_left_; int cut_right_; - bool special_filter_close_object_; + bool create_close_obstacle_if_depth_is_missing_; ros::Publisher cloudPub_; From 27dc3a1933ff7fd56460d068eb660f19d8d0c527 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Passot Date: Mon, 13 Jul 2015 17:23:07 -0700 Subject: [PATCH 28/32] Addressing pull request comments --- src/nodelets/obstacles_detection.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 02b68e94..ee1fc08d 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -213,36 +213,36 @@ private: // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the // grond normal angle (* 2.). - pcl::PointCloud::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits::min(), 1.); - pcl::PointCloud::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits::max()); + pcl::PointCloud::Ptr hypotheticalGroundCloud_near = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr hypotheticalGroundCloud_far = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits::max()); obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits::max()); //STEP 1. - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_front, + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_near, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); if(ground.get() && ground->size()) { - pcl::copyPointCloud(*hypotheticalGroundCloud_front, *ground, *groundCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_near, *ground, *groundCloud); } if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_front, *obstacles, *obstaclesNearFloorCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_near, *obstacles, *obstaclesNearFloorCloud); *obstaclesCloud += *obstaclesNearFloorCloud; } //STEP 2. - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_back, + rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_far, ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_); if(ground.get() && ground->size()) { pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_back, *ground, *groundCloud2); + pcl::copyPointCloud(*hypotheticalGroundCloud_far, *ground, *groundCloud2); *groundCloud += *groundCloud2; } @@ -250,7 +250,7 @@ private: if(obstacles.get() && obstacles->size()) { pcl::PointCloud::Ptr obstaclesNearFloorBackCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorBackCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_far, *obstacles, *obstaclesNearFloorBackCloud); *obstaclesCloud += *obstaclesNearFloorBackCloud; } From 0e4f7eaddc868197755d5b27892471b5c541b269 Mon Sep 17 00:00:00 2001 From: bibarz Date: Mon, 13 Jul 2015 18:15:09 -0700 Subject: [PATCH 29/32] rename --- src/nodelets/obstacles_detection.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index ee1fc08d..84ece7ad 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -198,9 +198,9 @@ private: if(obstacles.get() && obstacles->size()) { - pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud); - *obstaclesCloud += *obstaclesNearFloorCloud; + pcl::PointCloud::Ptr obstaclesFloorCloud(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesFloorCloud); + *obstaclesCloud += *obstaclesFloorCloud; } } @@ -218,7 +218,7 @@ private: obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits::max()); - //STEP 1. + // Part 1: segment floor and obstacles near the robot rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_near, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); @@ -230,12 +230,12 @@ private: if(obstacles.get() && obstacles->size()) { - pcl::PointCloud::Ptr obstaclesNearFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_near, *obstacles, *obstaclesNearFloorCloud); - *obstaclesCloud += *obstaclesNearFloorCloud; + pcl::PointCloud::Ptr obstaclesFloorCloud_near(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_near, *obstacles, *obstaclesFloorCloud_near); + *obstaclesCloud += *obstaclesFloorCloud_near; } - //STEP 2. + // Part 2: segment floor and obstacles far from the robot rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_far, ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_); @@ -249,9 +249,9 @@ private: if(obstacles.get() && obstacles->size()) { - pcl::PointCloud::Ptr obstaclesNearFloorBackCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_far, *obstacles, *obstaclesNearFloorBackCloud); - *obstaclesCloud += *obstaclesNearFloorBackCloud; + pcl::PointCloud::Ptr obstaclesFloorCloud_far(new pcl::PointCloud); + pcl::copyPointCloud(*hypotheticalGroundCloud_far, *obstacles, *obstaclesFloorCloud_far); + *obstaclesCloud += *obstaclesFloorCloud_far; } } From 68e9c6bc1638fc847a7306b7148859d1b2d02236 Mon Sep 17 00:00:00 2001 From: bibarz Date: Mon, 13 Jul 2015 18:23:56 -0700 Subject: [PATCH 30/32] recomment --- src/nodelets/obstacles_detection.cpp | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 84ece7ad..d6670195 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -181,12 +181,19 @@ private: hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - if (!simpleSegmentation_ && !optimizeForCloseObject_){ - //This is the default strategy - //The cloud is divided into two pointcloud based on reported Z and the position of the camera. + if (simpleSegmentation_){ + // If the option simple segmentation has been set to true, + // the floor is just the hypothetical ground cloud, simply + // cut off based on z + groundCloud = hypotheticalGroundCloud; + } + + else if (!optimizeForCloseObject_){ + // This is the default strategy + // The cloud is divided in two based on reported Z and the position of the camera. // One is the hypothetical ground cloud and the other one is the obstacles pointcloud. - //The algorithm then extracts (and remove) from the hypothetical ground cloud the detected obstacles, - //and add them to the obstacles pointcloud + // The algorithm then extracts (and removes) from the hypothetical ground cloud + // the detected obstacles, and adds them to the obstacles pointcloud rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); @@ -204,13 +211,14 @@ private: } } - if (!simpleSegmentation_ && optimizeForCloseObject_){ + + else if (optimizeForCloseObject_){ // If the option optimize for close object has been set to true, // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m // one for potential floor points further away than 1m. // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, // which allows to detect smaller objects, without increasing the number of false positive. - // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the + // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the // grond normal angle (* 2.). pcl::PointCloud::Ptr hypotheticalGroundCloud_near = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits::min(), 1.); @@ -255,14 +263,11 @@ private: } } - //If the option simple segmentation has been set to true, - //the floor is always the hypothetical ground cloud, with is a cut-off based on the z estimation of each point if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; - if (simpleSegmentation_) {pcl::toROSMsg(*hypotheticalGroundCloud, rosCloud);;} - else {pcl::toROSMsg(*groundCloud, rosCloud);} + pcl::toROSMsg(*groundCloud, rosCloud); rosCloud.header.stamp = cloudMsg->header.stamp; rosCloud.header.frame_id = frameId_; From c87bc63ecfa3f13c17f538156ee6250074833259 Mon Sep 17 00:00:00 2001 From: bibarz Date: Mon, 13 Jul 2015 18:26:07 -0700 Subject: [PATCH 31/32] recomment --- src/nodelets/obstacles_detection.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index d6670195..3bef24c4 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -181,14 +181,14 @@ private: hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - if (simpleSegmentation_){ + if (simpleSegmentation_) { // If the option simple segmentation has been set to true, // the floor is just the hypothetical ground cloud, simply // cut off based on z groundCloud = hypotheticalGroundCloud; } - else if (!optimizeForCloseObject_){ + else if (!optimizeForCloseObject_) { // This is the default strategy // The cloud is divided in two based on reported Z and the position of the camera. // One is the hypothetical ground cloud and the other one is the obstacles pointcloud. @@ -212,8 +212,8 @@ private: } - else if (optimizeForCloseObject_){ - // If the option optimize for close object has been set to true, + else { + // in this case optimizeForCloseObject_ is true: // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m // one for potential floor points further away than 1m. // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, From 48658d4e55549888bb8dde8c2bc42ec68ad5a533 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Jul 2015 18:29:05 -0400 Subject: [PATCH 32/32] Fixed backward compatibilities for obstacles_detection nodelet --- launch/tests/test_obstacles_detection.launch | 30 +++ src/nodelets/obstacles_detection.cpp | 224 ++++++++----------- src/nodelets/point_cloud_xyz.cpp | 8 +- 3 files changed, 127 insertions(+), 135 deletions(-) create mode 100644 launch/tests/test_obstacles_detection.launch diff --git a/launch/tests/test_obstacles_detection.launch b/launch/tests/test_obstacles_detection.launch new file mode 100644 index 00000000..acaf83bc --- /dev/null +++ b/launch/tests/test_obstacles_detection.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 3bef24c4..95d7196f 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -70,11 +70,9 @@ public: normalEstimationRadius_(0.05), groundNormalAngle_(M_PI_4), minClusterSize_(20), - maxFloorHeight_(-1), - maxObstaclesHeight_(1.5), + maxObstaclesHeight_(0.0), // if<=0.0 -> disabled waitForTransform_(false), - simpleSegmentation_(false), - optimizeForCloseObject_(true) + optimizeForCloseObjects_(false) {} virtual ~ObstaclesDetection() @@ -93,23 +91,21 @@ private: pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); - pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); - pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_); - pnh.param("optimize_for_close_object", optimizeForCloseObject_, optimizeForCloseObject_); + pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); groundPub_ = nh.advertise("ground", 1); obstaclesPub_ = nh.advertise("obstacles", 1); - - this->_lastFrameTime = ros::Time::now(); } void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { + ros::Time time = ros::Time::now(); + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0) { // no one wants the results @@ -140,128 +136,101 @@ private: pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *originalCloud); - //Even if the original cloud is empty, we need to publish the empty cloud, - //Otherwise, the aggregator of point cloud would wait indefinitely to get a valid pointcloud - if(originalCloud->size() == 0) - { - ROS_ERROR("Recieved empty point cloud!"); - if(groundPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*originalCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - groundPub_.publish(rosCloud); - } - - if(obstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*originalCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - obstaclesPub_.publish(rosCloud); - } - return; - } - //Common variables for all strategies - pcl::PointCloud::Ptr hypotheticalGroundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); pcl::IndicesPtr ground, obstacles; + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - ros::Time lasttime = ros::Time::now(); - - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxFloorHeight_); - obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); - - if (simpleSegmentation_) { - // If the option simple segmentation has been set to true, - // the floor is just the hypothetical ground cloud, simply - // cut off based on z - groundCloud = hypotheticalGroundCloud; - } - - else if (!optimizeForCloseObject_) { - // This is the default strategy - // The cloud is divided in two based on reported Z and the position of the camera. - // One is the hypothetical ground cloud and the other one is the obstacles pointcloud. - // The algorithm then extracts (and removes) from the hypothetical ground cloud - // the detected obstacles, and adds them to the obstacles pointcloud - - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud, - ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - - if(ground.get() && ground->size()) + if(originalCloud->size()) + { + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + if(maxObstaclesHeight_ > 0) { - pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); + originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); } - if(obstacles.get() && obstacles->size()) + if(originalCloud->size()) { - pcl::PointCloud::Ptr obstaclesFloorCloud(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesFloorCloud); - *obstaclesCloud += *obstaclesFloorCloud; + if(!optimizeForCloseObjects_) + { + // This is the default strategy + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud, + ground, + obstacles, + normalEstimationRadius_, + groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); + } + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud); + } + } + else + { + // in this case optimizeForCloseObject_ is true: + // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m + // one for potential floor points further away than 1m. + // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, + // which allows to detect smaller objects, without increasing the number of false positive. + // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the + // grond normal angle (* 2.). + + pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + + // Part 1: segment floor and obstacles near the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_near, + ground, + obstacles, + normalEstimationRadius_, + groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); + ground->clear(); + } + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); + obstacles->clear(); + } + + // Part 2: segment floor and obstacles far from the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_far, + ground, + obstacles, + 3.*normalEstimationRadius_, + 2.*groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); + *groundCloud += *groundCloud2; + } + + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); + *obstaclesCloud += *obstacles2; + } + } } - - } - - else { - // in this case optimizeForCloseObject_ is true: - // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m - // one for potential floor points further away than 1m. - // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, - // which allows to detect smaller objects, without increasing the number of false positive. - // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the - // grond normal angle (* 2.). - - pcl::PointCloud::Ptr hypotheticalGroundCloud_near = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits::min(), 1.); - pcl::PointCloud::Ptr hypotheticalGroundCloud_far = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits::max()); - - obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits::max()); - - // Part 1: segment floor and obstacles near the robot - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_near, - ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - - if(ground.get() && ground->size()) - { - pcl::copyPointCloud(*hypotheticalGroundCloud_near, *ground, *groundCloud); - } - - - if(obstacles.get() && obstacles->size()) - { - pcl::PointCloud::Ptr obstaclesFloorCloud_near(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_near, *obstacles, *obstaclesFloorCloud_near); - *obstaclesCloud += *obstaclesFloorCloud_near; - } - - // Part 2: segment floor and obstacles far from the robot - rtabmap::util3d::segmentObstaclesFromGround(hypotheticalGroundCloud_far, - ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_); - - if(ground.get() && ground->size()) - { - pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_far, *ground, *groundCloud2); - *groundCloud += *groundCloud2; - } - - - if(obstacles.get() && obstacles->size()) - { - pcl::PointCloud::Ptr obstaclesFloorCloud_far(new pcl::PointCloud); - pcl::copyPointCloud(*hypotheticalGroundCloud_far, *obstacles, *obstaclesFloorCloud_far); - *obstaclesCloud += *obstaclesFloorCloud_far; - } - } if(groundPub_.getNumSubscribers()) @@ -286,11 +255,7 @@ private: obstaclesPub_.publish(rosCloud); } - ros::Time curtime = ros::Time::now(); - - ros::Duration process_duration = curtime - lasttime; - ros::Duration between_frames = curtime - this->_lastFrameTime; - this->_lastFrameTime = curtime; + ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); } private: @@ -299,10 +264,8 @@ private: double groundNormalAngle_; int minClusterSize_; double maxObstaclesHeight_; - double maxFloorHeight_; bool waitForTransform_; - bool simpleSegmentation_; - bool optimizeForCloseObject_; + bool optimizeForCloseObjects_; tf::TransformListener tfListener_; @@ -310,7 +273,6 @@ private: ros::Publisher obstaclesPub_; ros::Subscriber cloudSub_; - ros::Time _lastFrameTime; }; PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet); diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index a72cd55e..6bed0638 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -66,13 +66,13 @@ public: decimation_(1), noiseFilterRadius_(0.0), noiseFilterMinNeighbors_(5), + cut_left_(0), + cut_right_(0), + create_close_obstacle_if_depth_is_missing_(false), approxSyncDepth_(0), approxSyncDisparity_(0), exactSyncDepth_(0), - exactSyncDisparity_(0), - cut_right_(0), - cut_left_(0), - create_close_obstacle_if_depth_is_missing_(false) + exactSyncDisparity_(0) {} virtual ~PointCloudXYZ()