Updated Info msg: changed localLoopClosureId for proximityDetectionId. point_cloud_xyzrgb nodelet: handling bgr and rgb encoding. Updated demo_find_object.launch with 0.11. MapsManager: Fixed grids generated with only one point.

launch/demo/demo_find_object.launch

@@ -1,8 +1,12 @@
 
 <launch>
 
-  <param name="use_sim_time" type="bool" value="True"/>
+  <!-- Choose visualization -->
+  <arg name="rviz" default="true" />
+  <arg name="rtabmapviz" default="false" /> 
 
+  <param name="use_sim_time" type="bool" value="True"/>
+  
   <!-- SLAM (robot side) -->
   <!-- args: "delete_db_on_start" and "udebug" -->
   <group ns="rtabmap">
@@ -10,7 +14,7 @@
 	  <param name="frame_id" type="string" value="base_footprint"/>
 	
 	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_laserScan" type="bool" value="true"/>
+	  <param name="subscribe_scan" type="bool" value="true"/>
 	
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
@@ -25,24 +29,39 @@
 	  <param name="queue_size" type="int" value="10"/>
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-	  <param name="RGBD/PoseScanMatching" type="string" value="true"/>             <!-- Do odometry correction with consecutive laser scans -->
+	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
+	  <param name="RGBD/ProximityBySpace" type="string" value="true"/>      <!-- Proximity detection (using estimated position) with locations in WM -->
-	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>    <!-- Local loop closure detection with locations in STM -->
+	  <param name="Mem/BadSignaturesIgnored" type="string" value="false"/>  <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
-	  <param name="Mem/BadSignaturesIgnored" type="string" value="false"/>      <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
+	  <param name="Reg/Strategy" type="string" value="1"/>                  <!-- Registration strategy: 0=visual, 1=ICP, 2=visual+ICP -->
-	  <param name="LccIcp/Type" type="string" value="2"/>                       <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
+	  <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
-	  <param name="LccIcp2/CorrespondenceRatio" type="string" value="0.9"/>
+	  <param name="Icp/Iterations" type="string" value="30"/>
-	  <param name="LccIcp2/MaxFitness" type="string" value="0.1"/>
+	  <param name="Icp/VoxelSize" type="string" value="0.025"/>
-	  <param name="LccIcp2/Iterations" type="string" value="100"/>
+	  <param name="Vis/MinInliers" type="string" value="10"/>                <!-- 3D visual words minimum inliers to accept loop closure -->
-	  <param name="LccIcp2/VoxelSize" type="string" value="0"/>
+	  <param name="Vis/MaxDepth" type="string" value="4.0"/>                 <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="LccBow/MinInliers" type="string" value="5"/>                 <!-- 3D visual words minimum inliers to accept loop closure -->
+	  <param name="Vis/InlierDistance" type="string" value="0.1"/>           <!-- 3D visual words correspondence distance -->
-	  <param name="LccBow/MaxDepth" type="string" value="4.0"/>                 <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="LccBow/InlierDistance" type="string" value="0.1"/>           <!-- 3D visual words correspondence distance -->
 	  <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
 	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+	  <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>   
     </node>
+    
+    <!-- Visualisation RTAB-Map -->
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+  	  <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_scan" type="bool" value="true"/>
+      <param name="frame_id" type="string" value="base_footprint"/>
+    
+      <remap from="rgb/image" to="/camera/data_throttled_image"/>
+      <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
+      <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
+      <remap from="scan" to="/base_scan"/>
+      <remap from="odom" to="/base_controller/odom"/>
+      
+      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="depth/image_transport" type="string" value="compressedDepth"/>
+    </node>
+    
   </group> 
   
   <!-- send AZIMUT 3 urdf to param server -->
@@ -51,10 +70,9 @@
   -->
   
   <!-- Visualisation -->
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
   
-  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
     <remap from="rgb/image"       to="/camera/data_throttled_image"/>
     <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
     <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>

msg/Info.msg

@@ -7,7 +7,7 @@ Header header
 
 int32 refId
 int32 loopClosureId
-int32 localLoopClosureId
+int32 proximityDetectionId
 
 geometry_msgs/Transform loopClosureTransform
 

src/MapsManager.cpp

@@ -39,6 +39,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudFrustumCulling_(false),
 		cloudNoiseFilteringRadius_(0.0),
 		cloudNoiseFilteringMinNeighbors_(5),
+		scanDecimation_(0),
 		scanVoxelSize_(0.0),
 		scanOutputVoxelized_(false),
 		projMaxGroundAngle_(45.0), // degrees
@@ -48,6 +49,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		gridSize_(0), // meters
 		gridEroded_(false),
 		gridUnknownSpaceFilled_(false),
+		gridMaxUnknownSpaceFilledRange_(6.0),
 		mapFilterRadius_(0.5),
 		mapFilterAngle_(30.0), // degrees
 		mapCacheCleanup_(true)
@@ -89,6 +91,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("grid_size", gridSize_, gridSize_); // m
 	pnh.param("grid_eroded", gridEroded_, gridEroded_);
 	pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
+	pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
 
 	// common map stuff
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
@@ -358,6 +361,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								{
 									scan = util3d::downsample(scan, scanDecimation_);
 								}
+
 								if(scanRequired || scanVoxelSize_ > 0.0)
 								{
 									pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
@@ -369,16 +373,24 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 											scan = util3d::laserScan2dFromPointCloud(*scanCloud);
 										}
 									}
+
 									if(scanRequired)
 									{
 										uInsert(scans_, std::make_pair(iter->first, scanCloud));
 									}
 								}
 							}
+
 							if(gridRequired && scan.type() == CV_32FC2)
 							{
 								cv::Mat ground, obstacles;
-								util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
+								util3d::occupancy2DFromLaserScan(
+										scan,
+										ground,
+										obstacles,
+										gridCellSize_,
+										data.id() < 0 || gridUnknownSpaceFilled_,
+										data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
 								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
 							}
 						}

src/MapsManager.h

@@ -85,6 +85,7 @@ private:
 	double gridSize_;
 	bool gridEroded_;
 	bool gridUnknownSpaceFilled_;
+	double gridMaxUnknownSpaceFilledRange_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;

src/MsgConversion.cpp

@@ -146,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
 	// rtabmap_ros::Info
 	stat.setRefImageId(info.refId);
 	stat.setLoopClosureId(info.loopClosureId);
-	stat.setLocalLoopClosureId(info.localLoopClosureId);
+	stat.setProximityDetectionId(info.proximityDetectionId);
 
 	stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
 
@@ -190,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 {
 	info.refId = stats.refImageId();
 	info.loopClosureId = stats.loopClosureId();
-	info.localLoopClosureId = stats.localLoopClosureId();
+	info.proximityDetectionId = stats.proximityDetectionId();
 
 	rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
 

src/nodelets/point_cloud_xyzrgb.cpp

@@ -173,7 +173,21 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
-			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr imagePtr;
+			if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+			{
+				imagePtr = cv_bridge::toCvShare(image);
+			}
+			else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+					image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				imagePtr = cv_bridge::toCvShare(image, "mono8");
+			}
+			else
+			{
+				imagePtr = cv_bridge::toCvShare(image, "bgr8");
+			}
+
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
 
 			image_geometry::PinholeCameraModel model;

src/rviz/InfoDisplay.cpp

@@ -51,8 +51,8 @@ void InfoDisplay::onInitialize()
 	this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
 	this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
 	this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
-	this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
-	this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
 
 	spinner_.start();
 }
@@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
 		boost::mutex::scoped_lock lock(info_mutex_);
 		if(msg->loopClosureId)
 		{
-			info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId);
+			info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
 			globalCount_ += 1;
 		}
-		else if(msg->localLoopClosureId)
+		else if(msg->proximityDetectionId)
 		{
-			info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId);
+			info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
 			localCount_ += 1;
 		}
 		else
@@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt )
 			this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
 			this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
 		}
-		this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
-		this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
 
 		for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
 		{