0.18.3: added apriltags2 support (input topic: tag_detections)

src/CoreWrapper.cpp

@@ -93,8 +93,10 @@ CoreWrapper::CoreWrapper() :
 		groundTruthFrameId_(""), // e.g., "world"
 		groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
 		configPath_(""),
-		odomDefaultAngVariance_(1.0),
-		odomDefaultLinVariance_(1.0),
+		odomDefaultAngVariance_(0.001),
+		odomDefaultLinVariance_(0.001),
+		landmarkDefaultAngVariance_(0.001),
+		landmarkDefaultLinVariance_(0.001),
 		waitForTransform_(true),
 		waitForTransformDuration_(0.2), // 200 ms
 		useActionForGoal_(false),
@@ -157,6 +159,8 @@ void CoreWrapper::onInit()
 	pnh.param("tf_tolerance",        tfTolerance, tfTolerance);
 	pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
 	pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
+	pnh.param("landmark_angular_variance", landmarkDefaultAngVariance_, landmarkDefaultAngVariance_);
+	pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_);
 	pnh.param("wait_for_transform",  waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
@@ -687,6 +691,9 @@ void CoreWrapper::onInit()
 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
 	globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
 	gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this);
+#ifdef WITH_APRILTAGS2_ROS
+	tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
+#endif
 }
 
 CoreWrapper::~CoreWrapper()
@@ -1292,6 +1299,22 @@ void CoreWrapper::commonDepthCallbackImpl(
 	}
 	gps_ = rtabmap::GPS();
 
+	//tag detections
+	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
+			tags_,
+			frameId_,
+			odomFrameId,
+			lastPoseStamp_,
+			tfListener_,
+			waitForTransform_?waitForTransformDuration_:0,
+			landmarkDefaultLinVariance_,
+			landmarkDefaultAngVariance_);
+	tags_.clear();
+	if(!landmarks.empty())
+	{
+		data.setLandmarks(landmarks);
+	}
+
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
 	{
@@ -1565,6 +1588,22 @@ void CoreWrapper::commonStereoCallback(
 	}
 	gps_ = rtabmap::GPS();
 
+	//tag detections
+	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
+			tags_,
+			frameId_,
+			odomFrameId,
+			lastPoseStamp_,
+			tfListener_,
+			waitForTransform_?waitForTransformDuration_:0,
+			landmarkDefaultLinVariance_,
+			landmarkDefaultAngVariance_);
+	tags_.clear();
+	if(!landmarks.empty())
+	{
+		data.setLandmarks(landmarks);
+	}
+
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
 	{
@@ -1785,6 +1824,22 @@ void CoreWrapper::commonLaserScanCallback(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
+	//tag detections
+	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
+			tags_,
+			frameId_,
+			odomFrameId,
+			lastPoseStamp_,
+			tfListener_,
+			waitForTransform_?waitForTransformDuration_:0,
+			landmarkDefaultLinVariance_,
+			landmarkDefaultAngVariance_);
+	tags_.clear();
+	if(!landmarks.empty())
+	{
+		data.setLandmarks(landmarks);
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1897,7 +1952,7 @@ void CoreWrapper::process(
 					memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
 					localizationPosePub_.publish(poseMsg);
 				}
-				std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
+				std::map<int, rtabmap::Transform> filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
 
 				// create a tmp signature with latest sensory data if latest signature was ignored
 				std::map<int, rtabmap::Signature> tmpSignature;
@@ -1909,9 +1964,9 @@ void CoreWrapper::process(
 					(!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
 				{
 					SensorData tmpData = data;
-					tmpData.setId(-1);
-					tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
-					filteredPoses.insert(std::make_pair(-1, mapToOdom_*odom));
+					tmpData.setId(0);
+					tmpSignature.insert(std::make_pair(0, Signature(0, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
+					filteredPoses.insert(std::make_pair(0, mapToOdom_*odom));
 				}
 
 				if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
@@ -1926,7 +1981,7 @@ void CoreWrapper::process(
 							nearestPoses.insert(*pter);
 						}
 					}
-					//add negative and make sure those on a planned path are not filtered
+					//add latest/zero and make sure those on a planned path are not filtered
 					std::set<int> onPath;
 					if(rtabmap_.getPath().size())
 					{
@@ -1935,7 +1990,7 @@ void CoreWrapper::process(
 					}
 					for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
 					{
-						if(iter->first < 0 || onPath.find(iter->first) != onPath.end())
+						if(iter->first == 0 || onPath.find(iter->first) != onPath.end())
 						{
 							nearestPoses.insert(*iter);
 						}
@@ -2113,6 +2168,22 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gps
 	}
 }
 
+#ifdef WITH_APRILTAGS2_ROS
+void CoreWrapper::tagDetectionsAsyncCallback(const apriltags2_ros::AprilTagDetectionArray & tagDetections)
+{
+	if(!paused_)
+	{
+		for(unsigned int i=0; i<tagDetections.detections.size(); ++i)
+		{
+			if(tagDetections.detections[i].id.size() == 1)
+			{
+				uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], tagDetections.detections[i].pose));
+			}
+		}
+	}
+}
+#endif
+
 
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {
@@ -2142,7 +2213,11 @@ void CoreWrapper::goalCommonCallback(
 
 	if(id > 0)
 	{
-		NODELET_INFO("Planning: set goal %d", id);
+		NODELET_INFO("Planning: set goal to node %d", id);
+	}
+	else if(id < 0)
+	{
+		NODELET_INFO("Planning: set goal to landmark %d", id);
 	}
 	else if(!pose.isNull())
 	{
@@ -2155,7 +2230,7 @@ void CoreWrapper::goalCommonCallback(
 	}
 
 	bool success = false;
-	if((id > 0 && rtabmap_.computePath(id, true)) ||
+	if((id != 0 && rtabmap_.computePath(id, true)) ||
 	   (!pose.isNull() && rtabmap_.computePath(pose)))
 	{
 		if(planningTime)
@@ -2231,6 +2306,10 @@ void CoreWrapper::goalCommonCallback(
 		{
 			NODELET_ERROR("Planning: Could not plan to node %d! The node is not in map's graph (look for warnings before this message for more details).", id);
 		}
+		else if(id < 0)
+		{
+			NODELET_ERROR("Planning: Could not plan to landmark %d! The landmark is not in map's graph (look for warnings before this message for more details).", id);
+		}
 		else
 		{
 			NODELET_ERROR("Planning: Node id should be > 0 !");
@@ -2280,7 +2359,7 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
 
 void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
 {
-	if(msg->node_id <= 0 && msg->node_label.empty())
+	if(msg->node_id == 0 && msg->node_label.empty())
 	{
 		NODELET_ERROR("Node id or label should be set!");
 		return;
@@ -2343,6 +2422,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	previousStamp_ = ros::Time(0);
 	globalPose_.header.stamp = ros::Time(0);
 	gps_ = rtabmap::GPS();
+	tags_.clear();
 	userDataMutex_.lock();
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
@@ -2404,6 +2484,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	userDataMutex_.unlock();
 	globalPose_.header.stamp = ros::Time(0);
 	gps_ = rtabmap::GPS();
+	tags_.clear();
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");
@@ -2658,8 +2739,8 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 		if(!req.graphOnly && mapsManager_.hasSubscribers())
 		{
-			std::map<int, Transform> filteredPoses = poses;
-			if(maxMappingNodes_ > 0 && poses.size()>1)
+			std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
+			if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
 			{
 				std::map<int, Transform> nearestPoses;
 				std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
@@ -3046,6 +3127,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 
 						markers.markers.push_back(marker);
 					}
+
 					// Add node ids
 					visualization_msgs::Marker marker;
 					marker.header.frame_id = mapFrameId_;

src/MapAssemblerNode.cpp

@@ -197,9 +197,9 @@ public:
 		{
 			Signature tmpS = nodes_.at(poses.rbegin()->first);
 			SensorData tmpData = tmpS.sensorData();
-			tmpData.setId(-1);
-			uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
-			poses.insert(std::make_pair(-1, poses.rbegin()->second));
+			tmpData.setId(0);
+			uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
+			poses.insert(std::make_pair(0, poses.rbegin()->second));
 		}
 
 		// Update maps

src/MapsManager.cpp

@@ -63,8 +63,8 @@ MapsManager::MapsManager() :
 		mapFilterRadius_(0.0),
 		mapFilterAngle_(30.0), // degrees
 		mapCacheCleanup_(true),
-		negativePosesIgnored_(true),
-		negativeScanEmptyRayTracing_(true),
+		alwaysUpdateMap_(false),
+		scanEmptyRayTracing_(true),
 		assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
 		assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
 		occupancyGrid_(new OccupancyGrid),
@@ -79,8 +79,30 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
 	pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
 	pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
-	pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
-	pnh.param("map_negative_scan_empty_ray_tracing", negativeScanEmptyRayTracing_, negativeScanEmptyRayTracing_);
+
+	if(pnh.hasParam("map_negative_poses_ignored"))
+	{
+		ROS_WARN("Parameter \"map_negative_poses_ignored\" has been "
+				"removed. Use \"map_always_update\" instead.");
+		if(!pnh.hasParam("map_always_update"))
+		{
+			bool negPosesIgnored;
+			pnh.getParam("map_negative_poses_ignored", negPosesIgnored);
+			alwaysUpdateMap_ = !negPosesIgnored;
+		}
+	}
+	pnh.param("map_always_update", alwaysUpdateMap_, alwaysUpdateMap_);
+
+	if(pnh.hasParam("map_negative_scan_empty_ray_tracing"))
+	{
+		ROS_WARN("Parameter \"map_negative_scan_empty_ray_tracing\" has been "
+				"removed. Use \"map_empty_ray_tracing\" instead.");
+		if(!pnh.hasParam("map_empty_ray_tracing"))
+		{
+			pnh.getParam("map_negative_scan_empty_ray_tracing", scanEmptyRayTracing_);
+		}
+	}
+	pnh.param("map_empty_ray_tracing", scanEmptyRayTracing_, scanEmptyRayTracing_);
 
 	if(pnh.hasParam("scan_output_voxelized"))
 	{
@@ -98,8 +120,8 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 	ROS_INFO("%s(maps): map_filter_radius          = %f", name.c_str(), mapFilterRadius_);
 	ROS_INFO("%s(maps): map_filter_angle           = %f", name.c_str(), mapFilterAngle_);
 	ROS_INFO("%s(maps): map_cleanup                = %s", name.c_str(), mapCacheCleanup_?"true":"false");
-	ROS_INFO("%s(maps): map_negative_poses_ignored = %s", name.c_str(), negativePosesIgnored_?"true":"false");
-	ROS_INFO("%s(maps): map_negative_scan_ray_tracing = %s", name.c_str(), negativeScanEmptyRayTracing_?"true":"false");
+	ROS_INFO("%s(maps): map_always_update          = %s", name.c_str(), alwaysUpdateMap_?"true":"false");
+	ROS_INFO("%s(maps): map_empty_ray_tracing      = %s", name.c_str(), scanEmptyRayTracing_?"true":"false");
 	ROS_INFO("%s(maps): cloud_output_voxelized     = %s", name.c_str(), cloudOutputVoxelized_?"true":"false");
 	ROS_INFO("%s(maps): cloud_subtract_filtering   = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
 	ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
@@ -302,7 +324,7 @@ void MapsManager::set2DMap(
 	//update cache in case the map should be updated
 	if(memory)
 	{
-		for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
 		{
 			std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
 			if(!uContains(gridMaps_, iter->first))
@@ -388,7 +410,7 @@ std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Trans
 }
 
 std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
-		const std::map<int, rtabmap::Transform> & poses,
+		const std::map<int, rtabmap::Transform> & posesIn,
 		const rtabmap::Memory * memory,
 		bool updateGrid,
 		bool updateOctomap,
@@ -434,6 +456,16 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		return std::map<int, rtabmap::Transform>();
 	}
 
+	// process only nodes (exclude landmarks)
+	std::map<int, rtabmap::Transform> poses;
+	if(posesIn.begin()->first < 0)
+	{
+		poses.insert(posesIn.lower_bound(0), posesIn.end());
+	}
+	else
+	{
+		poses = posesIn;
+	}
 	std::map<int, rtabmap::Transform> filteredPoses;
 
 	// update cache
@@ -445,17 +477,10 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			UDEBUG("Filter nodes...");
 			double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
 			filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
-			for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			if(poses.find(0) != poses.end())
 			{
-				if(iter->first <=0)
-				{
-					// make sure to keep latest data
-					filteredPoses.insert(*iter);
-				}
-				else
-				{
-					break;
-				}
+				// make sure to keep latest data
+				filteredPoses.insert(*poses.find(0));
 			}
 		}
 		else
@@ -463,19 +488,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			filteredPoses = poses;
 		}
 
-		if(negativePosesIgnored_)
+		if(alwaysUpdateMap_)
 		{
-			for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
-			{
-				if(iter->first <= 0)
-				{
-					filteredPoses.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
+			filteredPoses.erase(0);
 		}
 
 		bool longUpdate = false;
@@ -505,7 +520,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			if(!iter->second.isNull())
 			{
 				rtabmap::SensorData data;
-				if(updateGridCache && (iter->first < 0 || !uContains(gridMaps_, iter->first)))
+				if(updateGridCache && (iter->first == 0 || !uContains(gridMaps_, iter->first)))
 				{
 					UDEBUG("Data required for %d", iter->first);
 					std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
@@ -518,78 +533,35 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
 					}
 
-					if(data.id() != 0)
+					UDEBUG("Adding grid map %d to cache...", iter->first);
+					cv::Point3f viewPoint;
+					cv::Mat ground, obstacles, emptyCells;
+					if(iter->first > 0)
 					{
-						UDEBUG("Adding grid map %d to cache...", iter->first);
-						cv::Point3f viewPoint;
-						cv::Mat ground, obstacles, emptyCells;
-						if(iter->first > 0)
+						cv::Mat rgb, depth;
+						LaserScan scan;
+						bool generateGrid = data.gridCellSize() == 0.0f;
+						static bool warningShown = false;
+						if(occupancySavedInDB && generateGrid && !warningShown)
 						{
-							cv::Mat rgb, depth;
-							LaserScan scan;
-							bool generateGrid = data.gridCellSize() == 0.0f;
-							static bool warningShown = false;
-							if(occupancySavedInDB && generateGrid && !warningShown)
-							{
-								warningShown = true;
-								UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
-										"any occupancy grid output) but it cannot be found "
-										"in memory. For convenience, the occupancy "
-										"grid is regenerated. Make sure parameter \"%s\" is true to "
-										"avoid this warning for the next locations added to map. For older "
-										"locations already in database without an occupancy grid map, you can use the "
-										"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
-										"save them back in the database for next sessions. This warning is only shown once.",
-										data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
-							}
-							if(memory && occupancySavedInDB && generateGrid)
-							{
-								// if we are here, it is because we loaded a database with old nodes not having occupancy grid set
-								// try reload again
-								data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
-							}
-							data.uncompressData(
-									occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
-									occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
-									!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
-									0,
-									generateGrid?0:&ground,
-									generateGrid?0:&obstacles,
-									generateGrid?0:&emptyCells);
-
-							if(generateGrid)
-							{
-								Signature tmp(data);
-								tmp.setPose(iter->second);
-								occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
-								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
-								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
-							}
-							else
-							{
-								viewPoint = data.gridViewPoint();
-								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
-								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
-							}
+							warningShown = true;
+							UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
+									"any occupancy grid output) but it cannot be found "
+									"in memory. For convenience, the occupancy "
+									"grid is regenerated. Make sure parameter \"%s\" is true to "
+									"avoid this warning for the next locations added to map. For older "
+									"locations already in database without an occupancy grid map, you can use the "
+									"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
+									"save them back in the database for next sessions. This warning is only shown once.",
+									data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
 						}
-						else
+						if(memory && occupancySavedInDB && generateGrid)
 						{
-							// generate tmp occupancy grid for negative ids (assuming data is already uncompressed)
-							// For negative laser scans, fill empty space?
-							bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
-							Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
-
-							if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
-							{
-								ParametersMap parameters;
-								parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(negativeScanEmptyRayTracing_)));
-								occupancyGrid_->parseParameters(parameters);
-							}
-
-							cv::Mat rgb, depth;
-							LaserScan scan;
-							bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_);
-							data.uncompressData(
+							// if we are here, it is because we loaded a database with old nodes not having occupancy grid set
+							// try reload again
+							data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
+						}
+						data.uncompressData(
 								occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
 								occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
 								!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
@@ -598,38 +570,74 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								generateGrid?0:&obstacles,
 								generateGrid?0:&emptyCells);
 
-							if(generateGrid)
-							{
-								Signature tmp(data);
-								tmp.setPose(iter->second);
-								occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
-								uInsert(gridMaps_,  std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
-								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
-							}
-							else
-							{
-								viewPoint = data.gridViewPoint();
-								uInsert(gridMaps_,  std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
-								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
-							}
-
-							// put back
-							if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
-							{
-								ParametersMap parameters;
-								parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
-								occupancyGrid_->parseParameters(parameters);
-							}
+						if(generateGrid)
+						{
+							Signature tmp(data);
+							tmp.setPose(iter->second);
+							occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
+							uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
+							uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
+						}
+						else
+						{
+							viewPoint = data.gridViewPoint();
+							uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
+							uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
 						}
 					}
-					else if(memory)
+					else
 					{
-						ROS_ERROR("Data missing for node %d to update the maps", iter->first);
+						// generate tmp occupancy grid for latest id (assuming data is already uncompressed)
+						// For negative laser scans, fill empty space?
+						bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
+						Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
+
+						if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
+						{
+							ParametersMap parameters;
+							parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_)));
+							occupancyGrid_->parseParameters(parameters);
+						}
+
+						cv::Mat rgb, depth;
+						LaserScan scan;
+						bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_);
+						data.uncompressData(
+							occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
+							occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
+							!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
+							0,
+							generateGrid?0:&ground,
+							generateGrid?0:&obstacles,
+							generateGrid?0:&emptyCells);
+
+						if(generateGrid)
+						{
+							Signature tmp(data);
+							tmp.setPose(iter->second);
+							occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
+							uInsert(gridMaps_,  std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
+							uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
+						}
+						else
+						{
+							viewPoint = data.gridViewPoint();
+							uInsert(gridMaps_,  std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
+							uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
+						}
+
+						// put back
+						if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
+						{
+							ParametersMap parameters;
+							parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
+							occupancyGrid_->parseParameters(parameters);
+						}
 					}
 				}
 
 				if(updateGrid &&
-						(iter->first < 0 ||
+						(iter->first == 0 ||
 						  occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
 				{
 					std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
@@ -645,7 +653,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
 				if(updateOctomap &&
-						(iter->first < 0 ||
+						(iter->first == 0 ||
 						  octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
 				{
 					std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
@@ -811,7 +819,7 @@ void MapsManager::publishMaps(
 				   cloudObstaclesPub_.getNumSubscribers();
 		bool graphGroundChanged = updateGround;
 		bool graphObstacleChanged = updateObstacles;
-		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
 		{
 			std::map<int, Transform>::const_iterator jter;
 			if(updateGround)

src/MsgConversion.cpp

@@ -1233,6 +1233,69 @@ void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool c
 	}
 }
 
+rtabmap::Landmarks landmarksFromROS(
+		const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		tf::TransformListener & listener,
+		double waitForTransform,
+		double defaultLinVariance,
+		double defaultAngVariance)
+{
+	//tag detections
+	rtabmap::Landmarks landmarks;
+	for(std::map<int, geometry_msgs::PoseWithCovarianceStamped>::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
+	{
+		rtabmap::Transform baseToCamera = rtabmap_ros::getTransform(
+				frameId,
+				iter->second.header.frame_id,
+				iter->second.header.stamp,
+				listener,
+				waitForTransform);
+
+		if(baseToCamera.isNull())
+		{
+			ROS_ERROR("Cannot transform tag pose from \"%s\" frame to \"%s\" frame!",
+					iter->second.header.frame_id.c_str(), frameId.c_str());
+			continue;
+		}
+
+		rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.pose.pose);
+
+		if(!baseToTag.isNull())
+		{
+			// Correction of the global pose accounting the odometry movement since we received it
+			rtabmap::Transform correction = rtabmap_ros::getTransform(
+					frameId,
+					odomFrameId,
+					iter->second.header.stamp,
+					odomStamp,
+					listener,
+					waitForTransform);
+			if(!correction.isNull())
+			{
+				baseToTag = correction * baseToTag;
+			}
+			else
+			{
+				ROS_WARN("Could not adjust tag pose accordingly to latest odometry pose. "
+						"If odometry is small since it received the tag pose and "
+						"covariance is large, this should not be a problem.");
+			}
+			cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.pose.covariance.data()).clone();
+			if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
+			{
+				covariance = cv::Mat::eye(6,6,CV_64FC1);
+				covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance;
+				covariance(cv::Range(3,6), cv::Range(3,6)) *= defaultAngVariance;
+			}
+			landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, baseToTag, covariance)));
+		}
+	}
+	return landmarks;
+}
+
 rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,

src/OdometryROS.cpp

@@ -36,7 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <cv_bridge/cv_bridge.h>
 
-#include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/odometry/OdometryF2M.h>
 #include <rtabmap/core/odometry/OdometryF2F.h>
 #include <rtabmap/core/util3d.h>
@@ -78,7 +77,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	stereoParams_(stereoParams),
 	visParams_(visParams),
 	icpParams_(icpParams),
-	guessStamp_(0.0),
 	previousStamp_(0.0),
 	expectedUpdateRate_(0.0)
 {
@@ -448,8 +446,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
 	{
-		Transform previousPose = this->getTransform(guessFrameId_, frameId_, guessStamp_>0.0?ros::Time(guessStamp_):stamp);
 		guessCurrentPose = this->getTransform(guessFrameId_, frameId_, stamp);
+		Transform previousPose = guessPreviousPose_.isNull()?guessCurrentPose:guessPreviousPose_;
 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
 			if(guess_.isNull())
@@ -460,7 +458,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			{
 				guess_ = guess_ * previousPose.inverse() * guessCurrentPose;
 			}
-			if(guessStamp_>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
+			if(!guessPreviousPose_.isNull() && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
 			{
 				float x,y,z,roll,pitch,yaw;
 				guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -478,11 +476,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 						rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
 						tfBroadcaster_.sendTransform(correctionMsg);
 					}
-					guessStamp_ = stamp.toSec();
+					guessPreviousPose_ = guessCurrentPose;
 					return;
 				}
 			}
-			guessStamp_ = stamp.toSec();
+			guessPreviousPose_ = guessCurrentPose;
 		}
 		else
 		{
@@ -557,11 +555,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
 
 			//set velocity
-			bool setTwist = !odometry_->previousVelocityTransform().isNull();
+			bool setTwist = !odometry_->getVelocityGuess().isNull();
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
 				odom.twist.twist.linear.z = z;
@@ -757,7 +755,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 	NODELET_INFO( "visual_odometry: reset odom!");
 	odometry_->reset();
 	guess_.setNull();
-	guessStamp_ = 0.0;
+	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	this->flushCallbacks();
@@ -770,7 +768,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
 	guess_.setNull();
-	guessStamp_ = 0.0;
+	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	this->flushCallbacks();

src/SaveObjectsExample.cpp

@@ -140,7 +140,7 @@ public:
 
         if(nodeToObjects_.size())
         {
-            for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+            for(std::map<int, rtabmap::Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
             {
                 if(nodeToObjects_.find(iter->first) != nodeToObjects_.end())
                 {