From de861caf7d3051d635d164dd2501b3a66541f26f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 8 Jun 2023 10:27:17 -0700 Subject: [PATCH] point_cloud_xyz: use organized normals is voxel filtering is not set. obstacles_detection: support input organized clouds for fast normal segmentation. --- .../src/nodelets/obstacles_detection.cpp | 107 ++++++++++++------ rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 16 ++- 2 files changed, 83 insertions(+), 40 deletions(-) diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 9474bccc..8fe03fdd 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -54,7 +54,8 @@ public: waitForTransform_(false), mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()), warned_(false) - {} + { + } virtual ~ObstaclesDetection() {} @@ -235,31 +236,24 @@ private: projObstaclesPub_ = nh.advertise("proj_obstacles", 1); } - - - void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + template + void process( + typename pcl::PointCloud::Ptr & inputCloud, + const std_msgs::Header & header) { - ros::WallTime time = ros::WallTime::now(); - - if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) - { - // no one wants the results - return; - } - rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); try { if(waitForTransform_) { - if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + if(!tfListener_.waitForTransform(frameId_, header.frame_id, header.stamp, ros::Duration(1))) { - NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), header.frame_id.c_str()); return; } } tf::StampedTransform tmp; - tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + tfListener_.lookupTransform(frameId_, header.frame_id, header.stamp, tmp); localTransform = rtabmap_conversions::transformFromTF(tmp); } catch(tf::TransformException & ex) @@ -275,14 +269,14 @@ private: { if(waitForTransform_) { - if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1))) + if(!tfListener_.waitForTransform(mapFrameId_, frameId_, header.stamp, ros::Duration(1))) { NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str()); return; } } tf::StampedTransform tmp; - tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp); + tfListener_.lookupTransform(mapFrameId_, frameId_, header.stamp, tmp); pose = rtabmap_conversions::transformFromTF(tmp); } catch(tf::TransformException & ex) @@ -292,17 +286,7 @@ private: } } - UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, - uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str()); - - pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *inputCloud); - if(inputCloud->isOrganized()) - { - std::vector indices; - pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices); - } - else if(!inputCloud->is_dense && inputCloud->height == 1) + if(!inputCloud->is_dense && inputCloud->height == 1) { if(!warned_) { @@ -316,18 +300,30 @@ private: //Common variables for all strategies pcl::IndicesPtr ground, obstacles; - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); + typename pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + typename pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + typename pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); if(inputCloud->size()) { + pcl::IndicesPtr indices(new std::vector); + if(inputCloud->isOrganized()) + { + for(size_t i = 0; isize(); ++i) + { + if(pcl::isFinite(inputCloud->at(i))) + { + indices->push_back(i); + } + } + } + inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); pcl::IndicesPtr flatObstacles(new std::vector); - pcl::PointCloud::Ptr cloud = grid_.segmentCloud( + typename pcl::PointCloud::Ptr cloud = grid_.segmentCloud( inputCloud, - pcl::IndicesPtr(new std::vector), + indices, pose, cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()), ground, @@ -399,14 +395,14 @@ private: } else { - ROS_WARN("obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0); + ROS_WARN("obstacles_detection: Input cloud is empty!"); } if(groundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2 rosCloud; pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header = cloudMsg->header; + rosCloud.header = header; //publish the message groundPub_.publish(rosCloud); @@ -416,7 +412,7 @@ private: { sensor_msgs::PointCloud2 rosCloud; pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header = cloudMsg->header; + rosCloud.header = header; //publish the message obstaclesPub_.publish(rosCloud); @@ -426,12 +422,49 @@ private: { sensor_msgs::PointCloud2 rosCloud; pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.stamp = header.stamp; rosCloud.header.frame_id = frameId_; //publish the message projObstaclesPub_.publish(rosCloud); } + } + + void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + ros::WallTime time = ros::WallTime::now(); + + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) + { + // no one wants the results + return; + } + + UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, + uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str()); + + bool hasNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(cloudMsg->fields[i].name.compare("normal_x") == 0) + { + hasNormals = true; + break; + } + } + + pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr inputCloudNormal(new pcl::PointCloud); + if(hasNormals) + { + pcl::fromROSMsg(*cloudMsg, *inputCloudNormal); + process(inputCloudNormal, cloudMsg->header); + } + else + { + pcl::fromROSMsg(*cloudMsg, *inputCloud); + process(inputCloud, cloudMsg->header); + } NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); } diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index 1e8d12e9..c5038419 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -100,6 +100,9 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + int queueSize = 10; bool approxSync = true; std::string roiStr; @@ -116,7 +119,7 @@ private: pnh.param("normal_k", normalK_, normalK_); pnh.param("normal_radius", normalRadius_, normalRadius_); pnh.param("filter_nans", filterNaNs_, filterNaNs_); - pnh.param("roi_ratios", roiStr, roiStr); + pnh.param("roi_ratios", roiStr, roiStr); // [left, right, top, bottom] // Deprecated if(pnh.hasParam("cut_left")) @@ -282,7 +285,6 @@ private: minDepth_, indices.get()); processAndPublish(pclCloud, indices, depthMsg->header); - NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); } } @@ -361,7 +363,15 @@ private: if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f)) { //compute normals - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); + pcl::PointCloud::Ptr normals; + if(pclCloud->isOrganized()) + { + normals = rtabmap::util3d::computeFastOrganizedNormals(pclCloud, indices, normalRadius_, normalK_); + } + else + { + normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); + } pcl::PointCloud::Ptr pclCloudNormal(new pcl::PointCloud); pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal); if(filterNaNs_)