i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding comments

Browse Source
This commit is contained in:
Jean-Baptiste Passot
2015-07-13 15:04:53 -07:00
parent 1ab9eb1308
commit ce219a9a34
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+10 -12
View File
@@ -127,8 +127,6 @@ private:
return; return;
} }
} }
tf::StampedTransform tmp; tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp); localTransform = rtabmap_ros::transformFromTF(tmp);
@@ -139,10 +137,11 @@ private:
return; return;
} }
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *originalCloud); 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) if(originalCloud->size() == 0)
{ {
ROS_ERROR("Recieved empty point cloud!"); ROS_ERROR("Recieved empty point cloud!");
@@ -170,12 +169,7 @@ private:
return; return;
} }
//Common variables for all strategies
/////////////////////////////////////////////////////////////////////////////
pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::IndicesPtr ground, obstacles; pcl::IndicesPtr ground, obstacles;
@@ -184,7 +178,11 @@ private:
ros::Time lasttime = ros::Time::now(); ros::Time lasttime = ros::Time::now();
if (!simpleSegmentation_ && !optimizeForCloseObject_){ 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); originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::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 // For all other points, we use a biger normal estimation radius (* 3.) and a bigger tolerance for the
// grond normal angle (* 2.). // grond normal angle (* 2.).
originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.); pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max()); pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
@@ -264,13 +261,14 @@ private:
} }
else{ 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); originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_); hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
} }
if(groundPub_.getNumSubscribers()) if(groundPub_.getNumSubscribers())
{ {
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;