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

Adding an option to cut incoming depth

Browse Source
This commit is contained in:
JRombouts
2015-07-08 12:38:35 -07:00
parent 89123f57bf
commit cc139c6664
2 changed files with 26 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+9 -18
View File
@@ -157,55 +157,46 @@ private:
ros::Time lasttime = ros::Time::now();
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
if (!simpleSegmentation_){
ROS_ERROR("1-1");
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
ROS_ERROR("1-2");
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
ROS_ERROR("1-3");
if(ground.get() && ground->size())
{
pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
}
ROS_ERROR("1-4");
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
ROS_ERROR("1-5");
if(obstacles.get() && obstacles->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud);
*obstaclesCloud += *obstaclesNearFloorCloud;
}
ROS_ERROR("R 44444444444444444444444444444");
}
else{
ROS_ERROR("2-1");
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
ROS_ERROR("2-2");
}
/*
if(groundPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*groundCloud, rosCloud);
if (simpleSegmentation_) {pcl::toROSMsg(*hypotheticalGroundCloud, rosCloud);;}
else {pcl::toROSMsg(*groundCloud, rosCloud);}
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
groundPub_.publish(rosCloud);
}*/
}
if(obstaclesPub_.getNumSubscribers())
{
@@ -223,9 +214,9 @@ private:
ros::Duration process_duration = curtime - lasttime;
ros::Duration between_frames = curtime - this->_lastFrameTime;
this->_lastFrameTime = curtime;
std::stringstream buffer;
buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
//std::stringstream buffer;
//buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
//buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
//ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
}
src/nodelets/point_cloud_xyz.cpp
+17 -3
View File
@@ -69,7 +69,8 @@ public:
approxSyncDepth_(0),
approxSyncDisparity_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0)
exactSyncDisparity_(0),
cut_(0)
{}
virtual ~PointCloudXYZ()
@@ -99,6 +100,7 @@ private:
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("cut", cut_, cut_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
@@ -147,6 +149,18 @@ private:
if(cloudPub_.getNumSubscribers())
{
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv::Mat image=imageDepthPtr->image;
int rows = image.rows;
int cols = image.cols;
if (cut_>0){
cv::Mat pRoi = image(cv::Rect(0, 0, cut_, rows));
pRoi.setTo(cv::Scalar(0.));
}
else if (cut_<0){
cv::Mat pRoi = image(cv::Rect(cols+cut_, 0, -cut_, rows));
pRoi.setTo(cv::Scalar(0.));
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
@@ -157,13 +171,12 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepth(
imageDepthPtr->image,
image,
cx,
cy,
fx,
fy,
decimation_);
processAndPublish(pclCloud, depth->header);
}
}
@@ -245,6 +258,7 @@ private:
int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
int cut_;
ros::Publisher cloudPub_;