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

pointcloud_to_depthimage: added fill_iterations parameter. point_cloud_xyz[rgb]: added filter_nans parameter. convertScan3dMsg(): remove NaNs if input cloud is not dense.

Browse Source
This commit is contained in:
matlabbe
2017-09-18 21:57:22 -04:00
parent c452990fa5
commit ea6f39eb73
6 changed files with 71 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+16
View File
@@ -1505,12 +1505,20 @@ bool convertScan3dMsg(
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
}
@@ -1520,6 +1528,10 @@ bool convertScan3dMsg(
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
@@ -1538,6 +1550,10 @@ bool convertScan3dMsg(
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
src/nodelets/icp_odometry.cpp
+9
View File
@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
@@ -167,12 +168,20 @@ private:
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
{
src/nodelets/point_cloud_xyz.cpp
+11
View File
@@ -75,6 +75,7 @@ public:
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0f),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
exactSyncDepth_(0),
@@ -112,6 +113,7 @@ private:
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("normal_k", normalK_, normalK_);
pnh.param("normal_radius", normalRadius_, normalRadius_);
pnh.param("filter_nans", filterNaNs_, filterNaNs_);
pnh.param("roi_ratios", roiStr, roiStr);
// Deprecated
@@ -314,10 +316,18 @@ private:
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, rosCloud);
}
rosCloud.header.stamp = header.stamp;
@@ -337,6 +347,7 @@ private:
int noiseFilterMinNeighbors_;
int normalK_;
float normalRadius_;
bool filterNaNs_;
std::vector<float> roiRatios_;
ros::Publisher cloudPub_;
src/nodelets/point_cloud_xyzrgb.cpp
+17
View File
@@ -77,9 +77,12 @@ public:
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0f),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
approxSyncStereo_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0),
exactSyncStereo_(0)
{}
@@ -87,10 +90,14 @@ public:
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(approxSyncDisparity_)
delete approxSyncDisparity_;
if(approxSyncStereo_)
delete approxSyncStereo_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(exactSyncDisparity_)
delete exactSyncDisparity_;
if(exactSyncStereo_)
delete exactSyncStereo_;
}
@@ -114,6 +121,7 @@ private:
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("normal_k", normalK_, normalK_);
pnh.param("normal_radius", normalRadius_, normalRadius_);
pnh.param("filter_nans", filterNaNs_, filterNaNs_);
pnh.param("roi_ratios", roiStr, roiStr);
//parse roi (region of interest)
@@ -459,10 +467,18 @@ private:
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, rosCloud);
}
rosCloud.header.stamp = header.stamp;
@@ -482,6 +498,7 @@ private:
int noiseFilterMinNeighbors_;
int normalK_;
float normalRadius_;
bool filterNaNs_;
std::vector<float> roiRatios_;
rtabmap::ParametersMap stereoBMParameters_;
src/nodelets/pointcloud_to_depthimage.cpp
+9 -2
View File
@@ -62,6 +62,7 @@ public:
waitForTransform_(0.1),
fillHolesSize_ (0),
fillHolesError_(0.1),
fillIterations_(1),
decimation_(1),
approxSync_(0),
exactSync_(0)
@@ -95,6 +96,7 @@ private:
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
pnh.param("fill_holes_error", fillHolesError_, fillHolesError_);
pnh.param("fill_iterations", fillIterations_, fillIterations_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("approx", approx, approx);
@@ -113,6 +115,7 @@ private:
ROS_INFO(" wait_for_transform=%fs", waitForTransform_);
ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
ROS_INFO(" fill_holes_error=%f", fillHolesError_);
ROS_INFO(" fill_iterations=%d", fillIterations_);
ROS_INFO(" decimation=%d", decimation_);
image_transport::ImageTransport it(nh);
@@ -196,9 +199,12 @@ private:
cv_bridge::CvImage depthImage;
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0)
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
}
depthImage.header = cameraInfoMsg->header;
@@ -228,6 +234,7 @@ private:
double waitForTransform_;
int fillHolesSize_;
double fillHolesError_;
int fillIterations_;
int decimation_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
src/nodelets/rgbdicp_odometry.cpp
+9
View File
@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
@@ -335,12 +336,20 @@ private:
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
{