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

fixed #436

Browse Source
This commit is contained in:
matlabbe
2020-07-07 13:55:42 -04:00
parent 9ef1c1a609
commit c3c76a8bd3
3 changed files with 20 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/tests/test_pointxyzrgb.launch
+14 -1
View File
@@ -19,10 +19,23 @@
<remap from="rgb/image" to="rgb/image_out"/> <remap from="rgb/image" to="rgb/image_out"/>
<remap from="depth/image" to="depth/image_out"/> <remap from="depth/image" to="depth/image_out"/>
<remap from="rgb/camera_info" to="rgb/camera_info_out"/> <remap from="rgb/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud" /> <remap from="cloud" to="/voxel_cloud_xyzrgb" />
<param name="voxel_size" type="double" value="0.02"/> <param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/> <param name="decimation" type="int" value="2"/>
<param name="noise_filter_radius" type="double" value="0.05"/>
<param name="normal_k" type="int" value="6"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth/image_out"/>
<remap from="depth/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud_xyz" />
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/>
<param name="noise_filter_radius" type="double" value="0.05"/>
<param name="normal_k" type="int" value="6"/>
</node> </node>
</group> </group>
src/nodelets/point_cloud_xyz.cpp
+3 -2
View File
@@ -291,10 +291,11 @@ private:
if(indices->size() && voxelSize_ > 0.0) if(indices->size() && voxelSize_ > 0.0)
{ {
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
pclCloud->is_dense = true;
} }
// Do radius filtering after voxel filtering ( a lot faster) // Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{ {
if(pclCloud->is_dense) if(pclCloud->is_dense)
{ {
@@ -310,7 +311,7 @@ private:
} }
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
{ {
//compute normals //compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
src/nodelets/point_cloud_xyzrgb.cpp
+3 -2
View File
@@ -470,10 +470,11 @@ private:
if(indices->size() && voxelSize_ > 0.0) if(indices->size() && voxelSize_ > 0.0)
{ {
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
pclCloud->is_dense = true;
} }
// Do radius filtering after voxel filtering ( a lot faster) // Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{ {
if(pclCloud->is_dense) if(pclCloud->is_dense)
{ {
@@ -489,7 +490,7 @@ private:
} }
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
{ {
//compute normals //compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);