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

Addressing pull request comments

Browse Source
This commit is contained in:
Jean-Baptiste Passot
2015-07-13 17:18:13 -07:00
parent ce219a9a34
commit ef7c63ca20
2 changed files with 22 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+11 -22
View File
@@ -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<int>::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<int>::min(), maxFloorHeight_);
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(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<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -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<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 hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits<int>::min(), 1.);
pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits<int>::max());
pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(originalCloud_front, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(originalCloud_back, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x", 0.8, std::numeric_limits<int>::max());
//STEP 1.
@@ -254,20 +249,14 @@ private:
if(obstacles.get() && obstacles->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorCloud);
*obstaclesCloud += *obstaclesNearFloorCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorBackCloud(new pcl::PointCloud<pcl::PointXYZ>);
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<int>::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())
{
src/nodelets/point_cloud_xyz.cpp
+11 -9
View File
@@ -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<unsigned short>(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<unsigned short>(x,y) == 0){
pRoi.at<unsigned short>(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_;