MapCloud rviz: Added floor filtering parameter

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1805 f169173b-cf89-36c8-b27e-44dbe73f0c83

launch/config/azimut3_nav.rviz

@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
+        - /MapCloud1
         - /Info1
       Splitter Ratio: 0.674658
     Tree Height: 497
@@ -155,8 +156,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 8.06687
+        Max Value: 3.74695
-        Min Value: 2.84503
+        Min Value: -2.17552e-07
         Value: true
       Axis: X
       Channel Name: intensity
@@ -304,6 +305,7 @@ Visualization Manager:
       Download graph: false
       Download map: false
       Enabled: true
+      Filter floor (m): 0.1
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -375,22 +377,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rtabmap/OrbitOriented
-      Distance: 12.8402
+      Distance: 5.21912
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.423813
+        X: 0.112302
-        Y: 0.739184
+        Y: 0.0241702
-        Z: -0.899116
+        Z: -0.051079
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.869797
+      Pitch: 0.769797
       Target Frame: base_link
       Value: OrbitOriented (rtabmap)
-      Yaw: 3.42561
+      Yaw: 3.11563
     Saved: ~
 Window Geometry:
   Displays:

src/rviz/MapCloudDisplay.cpp

@@ -141,6 +141,13 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_voxel_size_->setMin( 0.0f );
 	cloud_voxel_size_->setMax( 1.0f );
 
+	cloud_filter_floor_height_ = new rviz::FloatProperty( "Filter floor (m)", 0.0f,
+										 "Filter the floor up to maximum height set here "
+										 "(only appropriate for 2D mapping).",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_filter_floor_height_->setMin( 0.0f );
+	cloud_filter_floor_height_->setMax( 999.0f );
+
 	node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
 										 "(Disabled=0) Only keep one node in the specified radius.",
 										 this, SLOT( updateCloudParameters() ), this );
@@ -314,6 +321,12 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 
 					cloud = util3d::transformPointCloud(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);
+					}
+
 					sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
 					pcl::toROSMsg(*cloud, *cloudMsg);
 					cloudMsg->header = map.header;

src/rviz/MapCloudDisplay.h

@@ -107,6 +107,7 @@ public:
 	rviz::IntProperty* cloud_decimation_;
 	rviz::FloatProperty* cloud_max_depth_;
 	rviz::FloatProperty* cloud_voxel_size_;
+	rviz::FloatProperty* cloud_filter_floor_height_;
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
 	rviz::BoolProperty* download_map_;