From e98d602c8f2281e90a043224cc0209c006cd62b5 Mon Sep 17 00:00:00 2001 From: Mohd Omama Date: Tue, 19 Jan 2021 20:02:52 +0530 Subject: [PATCH 1/2] Apply filtering in base link frame --- octomap_server/src/OctomapServer.cpp | 70 ++++++++++------------------ 1 file changed, 24 insertions(+), 46 deletions(-) diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index 01eb19a2..bee8af1f 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -260,24 +260,29 @@ bool OctomapServer::openFile(const std::string& filename){ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ ros::WallTime startTime = ros::WallTime::now(); - - // // ground filtering in base frame // PCLPointCloud pc; // input cloud for filtering and ground-detection pcl::fromROSMsg(*cloud, pc); - tf::StampedTransform sensorToWorldTf; + tf::StampedTransform sensorToWorldTf, sensorToBaseTf, baseToWorldTf; try { m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); + m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); + m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf); + m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); } catch(tf::TransformException& ex){ - ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); + ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback.\n" + "You need to set base_frame_id to apply filtering.\n" + "Also check if transforms exist between map, sensor and base frames."); return; } - Eigen::Matrix4f sensorToWorld; + Eigen::Matrix4f sensorToWorld, sensorToBase, baseToWorld; pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); + pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); + pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); // set up filter for height range, also removes NANs: @@ -294,55 +299,28 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr PCLPointCloud pc_ground; // segmented ground plane PCLPointCloud pc_nonground; // everything else - if (m_filterGroundPlane){ - tf::StampedTransform sensorToBaseTf, baseToWorldTf; - try{ - m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); - m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf); - m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); - - - }catch(tf::TransformException& ex){ - ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n" - "You need to set the base_frame_id or disable filter_ground."); - } - + // transform pointcloud from sensor frame to fixed robot frame + pcl::transformPointCloud(pc, pc, sensorToBase); + pass_x.setInputCloud(pc.makeShared()); + pass_x.filter(pc); + pass_y.setInputCloud(pc.makeShared()); + pass_y.filter(pc); + pass_z.setInputCloud(pc.makeShared()); + pass_z.filter(pc); - Eigen::Matrix4f sensorToBase, baseToWorld; - pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); - pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); - - // transform pointcloud from sensor frame to fixed robot frame - pcl::transformPointCloud(pc, pc, sensorToBase); - pass_x.setInputCloud(pc.makeShared()); - pass_x.filter(pc); - pass_y.setInputCloud(pc.makeShared()); - pass_y.filter(pc); - pass_z.setInputCloud(pc.makeShared()); - pass_z.filter(pc); + if (m_filterGroundPlane){ filterGroundPlane(pc, pc_ground, pc_nonground); - - // transform clouds to world frame for insertion - pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); - pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); - } else { - // directly transform to map frame: - pcl::transformPointCloud(pc, pc, sensorToWorld); - - // just filter height range: - pass_x.setInputCloud(pc.makeShared()); - pass_x.filter(pc); - pass_y.setInputCloud(pc.makeShared()); - pass_y.filter(pc); - pass_z.setInputCloud(pc.makeShared()); - pass_z.filter(pc); - + } + else { pc_nonground = pc; // pc_nonground is empty without ground segmentation pc_ground.header = pc.header; pc_nonground.header = pc.header; } + // transform clouds to world frame for insertion + pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); + pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground); From 84d04a6ff2c0265b9e743ee6b7d41909f378a27d Mon Sep 17 00:00:00 2001 From: Mohd Omama Date: Tue, 19 Jan 2021 20:33:46 +0530 Subject: [PATCH 2/2] feat: Dynamic clearing of map after filtering --- octomap_server/src/OctomapServer.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index bee8af1f..59c58f7d 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -305,17 +305,17 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr pass_x.filter(pc); pass_y.setInputCloud(pc.makeShared()); pass_y.filter(pc); - pass_z.setInputCloud(pc.makeShared()); - pass_z.filter(pc); - + if (m_filterGroundPlane){ + pass_z.setInputCloud(pc.makeShared()); + pass_z.filter(pc); filterGroundPlane(pc, pc_ground, pc_nonground); } else { - pc_nonground = pc; - // pc_nonground is empty without ground segmentation - pc_ground.header = pc.header; - pc_nonground.header = pc.header; + pass_z.setInputCloud(pc.makeShared()); + pass_z.filter(pc_nonground); + pass_z.setFilterLimitsNegative(true); + pass_z.filter(pc_ground); } // transform clouds to world frame for insertion