Updated how maps are built (the last sensor data are always integrated to maps, with unknown cells filled for laser scan)

src/CoreWrapper.cpp

@@ -888,13 +888,6 @@ void CoreWrapper::commonDepthCallback(
 			return;
 		}
 
-		// set maps manager laser scan range parameter
-		mapsManager_.setLaserScanParameters(
-				scanMsg->range_max,
-				scanMsg->angle_min,
-				scanMsg->angle_max,
-				scanMsg->angle_increment);
-
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
@@ -1000,13 +993,6 @@ void CoreWrapper::commonStereoCallback(
 			return;
 		}
 
-		// set maps manager laser scan range parameter
-		mapsManager_.setLaserScanParameters(
-				scanMsg->range_max,
-				scanMsg->angle_min,
-				scanMsg->angle_max,
-				scanMsg->angle_increment);
-
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
@@ -1233,14 +1219,24 @@ void CoreWrapper::process(
 
 			// Publish local graph, info
 			this->publishStats(stamp);
-			std::map<int, rtabmap::Transform> filteredPoses;
+			std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
 
+			// create a tmp signature with latest sensory data
+			std::map<int, rtabmap::Signature> tmpSignature;
+			SensorData tmpData = data;
+			tmpData.setId(-1);
+			tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, tmpData)));
+			filteredPoses.insert(std::make_pair(-1, odom));
+
+			// Update maps
 			filteredPoses = mapsManager_.updateMapCaches(
-					rtabmap_.getLocalOptimizedPoses(),
+					filteredPoses,
 					rtabmap_.getMemory(),
 					false,
 					false,
-					false);
+					false,
+					tmpSignature);
+
 			mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
 
 			// update goal if planning is enabled

src/MapsManager.cpp

@@ -43,11 +43,7 @@ MapsManager::MapsManager() :
 		gridUnknownSpaceFilled_(false),
 		mapFilterRadius_(0.5),
 		mapFilterAngle_(30.0), // degrees
-		mapCacheCleanup_(true),
+		mapCacheCleanup_(true)
-		laserScanMaxRange_(0),
-		laserScanMinAngle_(0),
-		laserScanMaxAngle_(0),
-		laserScanIncrement_(0)
 {
 
 	ros::NodeHandle nh;
@@ -90,10 +86,6 @@ void MapsManager::clear()
 	clouds_.clear();
 	projMaps_.clear();
 	gridMaps_.clear();
-	laserScanMaxRange_ = 0;
-	laserScanMinAngle_ = 0;
-	laserScanMaxAngle_ = 0;
-	laserScanIncrement_ = 0;
 }
 
 bool MapsManager::hasSubscribers() const
@@ -103,18 +95,6 @@ bool MapsManager::hasSubscribers() const
 			gridMapPub_.getNumSubscribers() != 0;
 }
 
-void MapsManager::setLaserScanParameters(
-		float maxRange,
-		float minAngle,
-		float maxAngle,
-		float increment)
-{
-	laserScanMaxRange_ = maxRange;
-	laserScanMinAngle_ = minAngle;
-	laserScanMaxAngle_ = maxAngle;
-	laserScanIncrement_ = increment;
-}
-
 std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
 {
 	if(mapFilterRadius_ > 0.0)
@@ -160,43 +140,45 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		{
 			double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
 			filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
+			if(poses.size() && poses.begin()->first < 0)
+			{
+				// make sure to keep latest data
+				filteredPoses.insert(*poses.begin());
+			}
 		}
 		else
 		{
 			filteredPoses = poses;
 		}
 
-
 		for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
 		{
 			if(!iter->second.isNull())
 			{
 				rtabmap::SensorData data;
-				bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
+				bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
-				bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
+				bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first));
-				bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
+				bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
+
 				if(rgbDepthRequired ||
 					depthRequired ||
 					scanRequired)
 				{
-					if(signatures.size())
+					std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
+					if(findIter != signatures.end())
 					{
-						std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
+						data = findIter->second.sensorData();
-						if(findIter != signatures.end())
-						{
-							data = findIter->second.sensorData();
-						}
 					}
-					else
+					else if(memory)
 					{
 						data = memory->getSignatureDataConst(iter->first);
 					}
 				}
 
-				if(data.id() > 0)
+				if(data.id() != 0)
 				{
-					if(!data.imageCompressed().empty() &&
+					if(!(data.imageCompressed().empty() && data.imageRaw().empty()) &&
-					   !data.depthOrRightCompressed().empty() &&
+					   !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) &&
 					   (data.cameraModels().size() || data.stereoCameraModel().isValid()))
 					{
 						// Which data should we decompress?
@@ -241,7 +223,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 
 						if(cloudRGB.get())
 						{
-							clouds_.insert(std::make_pair(iter->first, cloudRGB));
+							uInsert(clouds_, std::make_pair(iter->first, cloudRGB));
 						}
 
 						if(depthRequired)
@@ -272,19 +254,23 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 									util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
 								}
 							}
-							projMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+							uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
 						}
 
 						if(scanRequired)
 						{
 							cv::Mat ground, obstacles;
-							util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, gridUnknownSpaceFilled_);
+							util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_);
-							gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+							uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
 						}
 					}
 					else
 					{
-						ROS_ERROR("Local transform detected for node %d", iter->first);
+						ROS_ERROR("Some data missing for node %d to update the maps (image=%d, depth=%d, camera=%d)",
+								iter->first,
+								!(data.imageCompressed().empty() && data.imageRaw().empty())?1:0,
+							   !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0,
+							   (data.cameraModels().size() || data.stereoCameraModel().isValid())?1:0);
 					}
 				}
 			}
@@ -500,49 +486,6 @@ cv::Mat MapsManager::generateGridMap(
 			xMin, yMin,
 			gridSize_,
 			gridEroded_);
-
-	// Fill unknown space around the last pose
-	if(!map.empty() &&
-		laserScanMaxRange_ &&
-		laserScanMinAngle_ < laserScanMaxAngle_ &&
-		laserScanIncrement_ &&
-		poses.size())
-	{
-		const Transform & pose = poses.rbegin()->second;
-		float roll, pitch, yaw;
-		pose.getEulerAngles(roll, pitch, yaw);
-		cv::Point2i start((pose.x()-xMin)/gridCellSize_ + 0.5f, (pose.y()-yMin)/gridCellSize_ + 0.5f);
-
-		//rotate counterclockwise 180 degrees at the computed step "a" degrees
-		cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(laserScanIncrement_), -sin(laserScanIncrement_),
-													 sin(laserScanIncrement_), cos(laserScanIncrement_));
-
-		cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F);
-		origin.at<float>(0) = pose.x();
-		origin.at<float>(1) = pose.y();
-		endFirst.at<float>(0) = laserScanMaxRange_;
-		endFirst.at<float>(1) = 0;
-
-		yaw += laserScanMinAngle_;
-		cv::Mat initRotation = (cv::Mat_<float>(2,2) << cos(yaw), -sin(yaw),
-														 sin(yaw), cos(yaw));
-
-		cv::Mat endCurrent = initRotation*endFirst + origin;
-		for(float a=laserScanMinAngle_; a<=laserScanMaxAngle_; a+=laserScanIncrement_)
-		{
-			cv::Point2i end((endCurrent.at<float>(0)-xMin)/gridCellSize_ + 0.5f, (endCurrent.at<float>(1)-yMin)/gridCellSize_ + 0.5f);
-			//end must be inside the grid
-			end.x = end.x < 0?0:end.x;
-			end.x = end.x >= map.cols?map.cols-1:end.x;
-			end.y = end.y < 0?0:end.y;
-			end.y = end.y >= map.rows?map.rows-1:end.y;
-			util3d::rayTrace(start, end, map, true); // trace free space
-
-			// next point
-			endCurrent = rotation*(endCurrent - origin) + origin;
-		}
-	}
-
 	return map;
 }
 

src/MapsManager.h

@@ -59,8 +59,6 @@ public:
 			float & yMin,
 			float & gridCellSize);
 
-	void setLaserScanParameters(float maxRange, float minAngle, float maxAngle, float increment);
-
 #ifdef WITH_OCTOMAP
 	octomap::OcTree * createOctomap(const std::map<int, rtabmap::Transform> & poses);
 #endif
@@ -82,11 +80,6 @@ private:
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;
 
-	float laserScanMaxRange_;
-	float laserScanMinAngle_;
-	float laserScanMaxAngle_;
-	float laserScanIncrement_;
-
 	ros::Publisher cloudMapPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;