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

New option for detecting close objects and other hacks for closer objects

Browse Source
This commit is contained in:
JRombouts
2015-07-10 17:01:08 -07:00
parent 59986a8066
commit f75744217c
2 changed files with 82 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+54 -2
View File
@@ -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<int>::min(), maxFloorHeight_);
/*
if (!simpleSegmentation_){
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_);
@@ -203,8 +205,58 @@ private:
*obstaclesCloud += *obstaclesNearFloorCloud;
}
}*/
if (!simpleSegmentation_){
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(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_);
//STEP 1.
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(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<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*hypotheticalGroundCloud_front, *obstacles, *obstaclesNearFloorCloud);
*obstaclesCloud += *obstaclesNearFloorCloud;
}
//STEP 2.
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud_back,
ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_);
if(ground.get() && ground->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*hypotheticalGroundCloud_back, *ground, *groundCloud2);
*groundCloud += *groundCloud2;
}
if(obstacles.get() && obstacles->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorCloud);
*obstaclesCloud += *obstaclesNearFloorCloud;
}
}
else{
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_);
}
src/nodelets/point_cloud_xyz.cpp
+28 -1
View File
@@ -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<unsigned short>(x,y) == 0){
pRoi.at<unsigned short>(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_;