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

ros-pkg: updated to rtabmap trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1922 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-24 16:59:55 +00:00
parent 1ef71dd5af
commit bef004eaa3
5 changed files with 58 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapAssemblerNode.cpp
+9 -9
View File
@@ -169,14 +169,14 @@ public:
if(cloudMaxDepth_ > 0)
{
cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloudMaxDepth_);
}
if(cloudVoxelSize_ > 0)
{
cloud = util3d::voxelize(cloud, cloudVoxelSize_);
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloudVoxelSize_);
}
cloud = util3d::transformPointCloud(cloud, localTransform);
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud));
@@ -185,7 +185,7 @@ public:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(maxHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
}
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_))
@@ -209,7 +209,7 @@ public:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
if(scanVoxelSize_ > 0)
{
cloud = util3d::voxelize(cloud, scanVoxelSize_);
cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_);
}
scans_.insert(std::make_pair(msg->depth2DIDs[i], cloud));
@@ -238,7 +238,7 @@ public:
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = rgbClouds_.find(iter->first);
if(jter != rgbClouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZRGB>(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
@@ -247,7 +247,7 @@ public:
{
if(cloudVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud,cloudVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -268,7 +268,7 @@ public:
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
if(jter != scans_.end())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZ>(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
@@ -277,7 +277,7 @@ public:
{
if(scanVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
assembledCloud = util3d::voxelize<pcl::PointXYZ>(assembledCloud, scanVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
src/OdometryROS.cpp
+1 -1
View File
@@ -328,7 +328,7 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud(cloud, pose);
cloudTransformed = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
src/nodelets/point_cloud_xyz.cpp
+43 -9
View File
@@ -58,7 +58,12 @@ namespace rtabmap
class PointCloudXYZ : public nodelet::Nodelet
{
public:
PointCloudXYZ() : voxelSize_(0.0), decimation_(1) {}
PointCloudXYZ() :
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5)
{}
virtual ~PointCloudXYZ()
{
@@ -76,6 +81,8 @@ private:
pnh.param("queue_size", queueSize, queueSize);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
sync_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
@@ -118,7 +125,7 @@ private:
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepth(
pclCloud = util3d::cloudFromDepth(
imageDepthPtr->image,
cx,
cy,
@@ -126,9 +133,17 @@ private:
fy,
decimation_);
if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
}
//*********************
@@ -149,14 +164,22 @@ private:
const stereo_msgs::DisparityImageConstPtr& disparityMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
ROS_ERROR("Input type must be disparity=32FC1");
ROS_ERROR("Input type must be disparity=32FC1 or 16SC1");
return;
}
// sensor_msgs::image_encodings::TYPE_32FC1
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
cv::Mat disparity;
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
else
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
if(cloudPub_.getNumSubscribers())
{
@@ -166,7 +189,7 @@ private:
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDisparity(
pclCloud = util3d::cloudFromDisparity(
disparity,
cx,
cy,
@@ -174,9 +197,17 @@ private:
disparityMsg->T,
decimation_);
if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
}
//*********************
@@ -197,6 +228,9 @@ private:
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
ros::Publisher cloudPub_;
image_transport::SubscriberFilter imageDepthSub_;
src/nodelets/point_cloud_xyzrgb.cpp
+1 -1
View File
@@ -135,7 +135,7 @@ private:
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
}
//*********************
src/rviz/MapCloudDisplay.cpp
+4 -4
View File
@@ -319,19 +319,19 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
}
if(cloud_max_depth_->getFloat() > 0.0f)
{
cloud = util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
}
if(cloud_voxel_size_->getFloat() > 0.0f)
{
cloud = util3d::voxelize(cloud, cloud_voxel_size_->getFloat());
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
}
cloud = util3d::transformPointCloud(cloud, localTransform);
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
// do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f)
{
cloud = util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);