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

Added roi_ratios to point_cloud_xyzrgb nodelet

Browse Source
This commit is contained in:
matlabbe
2016-05-19 14:43:34 -04:00
parent db4e1419a4
commit 440e8acdc6
3 changed files with 95 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+1 -1
View File
@@ -305,7 +305,7 @@ private:
projObstaclesPub_.publish(rosCloud);
}
//NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
}
private:
src/nodelets/point_cloud_xyz.cpp
+32 -22
View File
@@ -127,31 +127,34 @@ private:
//parse roi (region of interest)
roiRatios_.resize(4, 0);
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
if(!roiStr.empty())
{
ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
}
@@ -202,22 +205,25 @@ private:
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
cv::Mat image(imageDepthPtr->image, roi);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepth(
image,
cv::Mat(imageDepthPtr->image, roi),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
model.fx(),
model.fy(),
decimation_);
processAndPublish(pclCloud, depth->header);
NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
}
}
@@ -244,6 +250,8 @@ private:
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv::Rect roi = rtabmap::Feature2D::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
@@ -255,6 +263,8 @@ private:
decimation_);
processAndPublish(pclCloud, disparityMsg->header);
NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
}
}
src/nodelets/point_cloud_xyzrgb.cpp
+62 -10
View File
@@ -55,6 +55,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap_ros
{
@@ -95,6 +98,7 @@ private:
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
@@ -103,6 +107,40 @@ private:
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("roi_ratios", roiStr, roiStr);
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
}
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
@@ -175,6 +213,8 @@ private:
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
@@ -194,23 +234,25 @@ private:
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
imagePtr->image,
imageDepthPtr->image,
cx,
cy,
fx,
fy,
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
model.fx(),
model.fy(),
decimation_);
processAndPublish(pclCloud, imagePtr->header);
NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
}
}
@@ -234,6 +276,8 @@ private:
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
@@ -246,6 +290,11 @@ private:
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
{
ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
@@ -254,6 +303,8 @@ private:
decimation_);
processAndPublish(pclCloud, imageLeft->header);
NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
}
}
@@ -302,6 +353,7 @@ private:
int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
std::vector<float> roiRatios_;
ros::Publisher cloudPub_;