Added Image display in rgbd.rviz

rviz plugin:  fixed assertion on Ogre::AxisAlignedBox: Assertion "(min.x <= max.x && min.y <= max.y && min.z <= max.z) && The minimum corner of the box must be less than or equal to maximum corner"
rviz plugin: added node_filtering_radius and node_filtering_angle properties for cloud filtering
rtabmap-ros: added set_mode_localization and set_mode_mapping services

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

launch/config/rgbd.rviz

@@ -6,8 +6,9 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
+        - /MapCloud1/Status1
       Splitter Ratio: 0.411058
-    Tree Height: 518
+    Tree Height: 422
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -81,10 +82,6 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        base_footprint:
-          Value: true
-        base_laser_link:
-          Value: true
         base_link:
           Value: true
         camera_depth_frame:
@@ -101,22 +98,6 @@ Visualization Manager:
           Value: true
         odom:
           Value: true
-        wheelLB_linkWheel_link:
-          Value: true
-        wheelLB_wheel_link:
-          Value: true
-        wheelLF_linkWheel_link:
-          Value: true
-        wheelLF_wheel_link:
-          Value: true
-        wheelRB_linkWheel_link:
-          Value: true
-        wheelRB_wheel_link:
-          Value: true
-        wheelRF_linkWheel_link:
-          Value: true
-        wheelRF_wheel_link:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -125,28 +106,13 @@ Visualization Manager:
       Tree:
         map:
           odom:
-            base_footprint:
+            base_link:
-              base_link:
+              camera_link:
-                base_laser_link:
+                camera_depth_frame:
-                  {}
+                  camera_depth_optical_frame:
-                camera_link:
-                  camera_depth_frame:
-                    camera_depth_optical_frame:
-                      {}
-                  camera_rgb_frame:
-                    camera_rgb_optical_frame:
-                      {}
-                wheelLB_linkWheel_link:
-                  wheelLB_wheel_link:
                     {}
-                wheelLF_linkWheel_link:
+                camera_rgb_frame:
-                  wheelLF_wheel_link:
+                  camera_rgb_optical_frame:
-                    {}
-                wheelRB_linkWheel_link:
-                  wheelRB_wheel_link:
-                    {}
-                wheelRF_linkWheel_link:
-                  wheelRF_wheel_link:
                     {}
       Update Interval: 0
       Value: true
@@ -183,9 +149,20 @@ Visualization Manager:
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
-      Enabled: true
+      Enabled: false
       Name: Map
       Topic: /rtabmap/grid_map
+      Value: false
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_rect_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -198,7 +175,7 @@ Visualization Manager:
       Class: rtabmap/MapCloud
       Cloud decimation: 4
       Cloud max depth (m): 4
-      Cloud voxel size (m): 0.02
+      Cloud voxel size (m): 0.01
       Color: 255; 255; 255
       Color Transformer: RGB8
       Download map: false
@@ -209,6 +186,8 @@ Visualization Manager:
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0.1
       Position Transformer: XYZ
       Size (Pixels): 3
       Size (m): 0.01
@@ -237,30 +216,32 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 12.2587
+      Distance: 6.35253
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 2.34873
+        X: 1.14572
-        Y: 0.212305
+        Y: 0.168097
-        Z: -2.50326
+        Z: -1.13136
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.939797
+      Pitch: 0.879797
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 3.45419
+      Yaw: 3.73418
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 691
+  Height: 824
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b200000295fc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000295000000dd00fffffffb0000000a0049006d00610067006501000002b1000000c70000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003410000029500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -270,5 +251,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1273
-  X: 121
+  X: 100
-  Y: 201
+  Y: 115

src/CoreWrapper.cpp

@@ -174,6 +174,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
 	resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
 	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
+	setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
+	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
 	getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -617,6 +619,24 @@ bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Emp
 	return true;
 }
 
+bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("rtabmap: Set localization mode");
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
+	rtabmap_.parseParameters(parameters);
+	return true;
+}
+
+bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("rtabmap: Set mapping mode");
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "true"));
+	rtabmap_.parseParameters(parameters);
+	return true;
+}
+
 bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep)
 {
 	ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",

src/CoreWrapper.h

@@ -76,6 +76,8 @@ private:
 	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
@@ -137,6 +139,8 @@ private:
 	ros::ServiceServer pauseSrv_;
 	ros::ServiceServer resumeSrv_;
 	ros::ServiceServer triggerNewMapSrv_;
+	ros::ServiceServer setModeLocalizationSrv_;
+	ros::ServiceServer setModeMappingSrv_;
 	ros::ServiceServer getMapDataSrv_;
 
 	boost::thread* transformThread_;

src/rviz/MapCloudDisplay.cpp

@@ -35,6 +35,7 @@ namespace rtabmap
 
 MapCloudDisplay::CloudInfo::CloudInfo() :
 		manager_(0),
+		pose_(Transform::getIdentity()),
 		scene_node_(0)
 {}
 
@@ -108,12 +109,24 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_max_depth_->setMin( 0.0f );
 	cloud_max_depth_->setMax( 999.0f );
 
-	cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.02f,
+	cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f,
 										 "Voxel size of the generated clouds.",
 										 this, SLOT( updateCloudParameters() ), this );
 	cloud_voxel_size_->setMin( 0.0f );
 	cloud_voxel_size_->setMax( 1.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 );
+	node_filtering_radius_->setMin( 0.0f );
+	node_filtering_radius_->setMax( 10.0f );
+
+	node_filtering_angle_ = new rviz::FloatProperty( "Node filtering angle (degrees)", 30.0f,
+										 "(Disabled=0) Only keep one node in the specified angle in the filtering radius.",
+										 this, SLOT( updateCloudParameters() ), this );
+	node_filtering_angle_->setMin( 0.0f );
+	node_filtering_angle_->setMax( 359.0f );
+
 	download_map_ = new rviz::BoolProperty( "Download map", false,
 										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
 										 this, SLOT( downloadMap() ), this );
@@ -279,6 +292,13 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 		poses.insert(std::make_pair(map.poseIDs[i], transformFromPoseMsg(map.poses[i])));
 	}
 
+	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
+	{
+		poses = util3d::radiusPosesFiltering(poses,
+				node_filtering_radius_->getFloat(),
+				node_filtering_angle_->getFloat()*CV_PI/180.0);
+	}
+
 	{
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_ = poses;
@@ -512,7 +532,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 
 				cloud_info->manager_ = context_->getSceneManager();
 
-				cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
+				cloud_info->scene_node_ = scene_node_->createChildSceneNode();
 
 				cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
 
@@ -548,6 +568,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 	}
 
 	int totalPoints = 0;
+	int totalNodesShown = 0;
 	{
 		// update poses
 		boost::mutex::scoped_lock lock(current_map_mutex_);
@@ -560,23 +581,27 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 				{
 					totalPoints += cloudInfoIt->second->transformed_points_.size();
 					cloudInfoIt->second->pose_ = it->second;
-					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, cloudInfoIt->second->position_, cloudInfoIt->second->orientation_))
+					Ogre::Vector3 framePosition;
+					Ogre::Quaternion frameOrientation;
+					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation))
 					{
-						// Set pose
+						// Multiply frame with pose
 						Ogre::Matrix4 frameTransform;
-						frameTransform.makeTransform( cloudInfoIt->second->position_, Ogre::Vector3(1,1,1), cloudInfoIt->second->orientation_ );
+						frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation);
 						const rtabmap::Transform & p = cloudInfoIt->second->pose_;
 						Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
 										 p[4], p[5], p[6], p[7],
 										 p[8], p[9], p[10], p[11],
 										 0, 0, 0, 1);
 						frameTransform = frameTransform * pose;
-						cloudInfoIt->second->position_ = frameTransform.getTrans();
+						Ogre::Vector3 posePosition = frameTransform.getTrans();
-						cloudInfoIt->second->orientation_ = frameTransform.extractQuaternion();
+						Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion();
+						poseOrientation.normalise();
 
-						cloudInfoIt->second->scene_node_->setPosition(cloudInfoIt->second->position_);
+						cloudInfoIt->second->scene_node_->setPosition(posePosition);
-						cloudInfoIt->second->scene_node_->setOrientation(cloudInfoIt->second->orientation_);
+						cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
 						cloudInfoIt->second->scene_node_->setVisible(true);
+						++totalNodesShown;
 					}
 					else
 					{
@@ -596,12 +621,8 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 		}
 	}
 
-	std::stringstream ss;
+	this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
-	ss << totalPoints;
+	this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
-	this->setStatusStd(rviz::StatusProperty::Ok, "Points", ss.str());
-	std::stringstream ss2;
-	ss2 << cloud_infos_.size();
-	this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", ss2.str());
 }
 
 void MapCloudDisplay::reset()

src/rviz/MapCloudDisplay.h

@@ -63,9 +63,6 @@ public:
 		boost::shared_ptr<rviz::PointCloud> cloud_;
 
 		std::vector<rviz::PointCloud::Point> transformed_points_;
-
-		Ogre::Quaternion orientation_;
-		Ogre::Vector3 position_;
 	};
 	typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
 
@@ -84,6 +81,8 @@ public:
 	rviz::IntProperty* cloud_decimation_;
 	rviz::FloatProperty* cloud_max_depth_;
 	rviz::FloatProperty* cloud_voxel_size_;
+	rviz::FloatProperty* node_filtering_radius_;
+	rviz::FloatProperty* node_filtering_angle_;
 	rviz::BoolProperty* download_map_;
 
 public Q_SLOTS: