Parameters: added Grid/FullUpdate (default true). Updated how occupancy grid is updated after loop closure. OctoMap: added tree depth argument when creating 2d map, added full update argument on constructor (default false). MainWindow: using OccupancyGrid object instead of keeping in cache local grids (we can have actual time to update the global grid).

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -45,6 +45,8 @@ public:
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
+	bool isFullUpdate() const {return fullUpdate_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -104,6 +106,7 @@ private:
 	bool scan2dUnknownSpaceFilled_;
 	double scan2dMaxUnknownSpaceFilledRange_;
 	bool projRayTracing_;
+	bool fullUpdate_;
 
 	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
 	cv::Mat map_;

corelib/include/rtabmap/core/OctoMap.h

@@ -57,7 +57,7 @@ public:
 
 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
+	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
@@ -81,7 +81,8 @@ public:
 			float & xMin,
 			float & yMin,
 			float & gridCellSize,
-			float minGridSize);
+			float minGridSize = 0.0f,
+			unsigned int treeDepth = 0);
 
 	bool writeBinary(const std::string & path);
 
@@ -97,6 +98,7 @@ private:
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
 	bool hasColor_;
+	bool fullUpdate_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -505,6 +505,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
 	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   4.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
 	RTABMAP_PARAM(Grid, ProjRayTracing,               bool,   true,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
+	RTABMAP_PARAM(Grid, FullUpdate,                   bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
 
 public:
 	virtual ~Parameters();

corelib/include/rtabmap/core/util3d_mapping.h

@@ -116,6 +116,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
+cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
+
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 		const typename pcl::PointCloud<PointT> & cloud);

corelib/src/OccupancyGrid.cpp

@@ -65,6 +65,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
 	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
 	projRayTracing_(Parameters::defaultGridProjRayTracing()),
+	fullUpdate_(Parameters::defaultGridFullUpdate()),
 	xMin_(0.0f),
 	yMin_(0.0f)
 {
@@ -131,6 +132,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
 	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
 	Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
+	Parameters::parse(parameters, Parameters::kGridFullUpdate(), fullUpdate_);
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -383,7 +385,7 @@ void OccupancyGrid::addToCache(
 		const cv::Mat & obstacles)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 }
 
 void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
@@ -391,7 +393,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	UTimer timer;
 	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
 
-	float margin = cellSize_*10.0f+footprintRadius;
+	float margin = cellSize_*10.0f+(footprintRadius>cellSize_*1.5f?float(int(footprintRadius/cellSize_)+1):0.0f)*cellSize_;
 
 	float minX=-minMapSize/2.0f;
 	float minY=-minMapSize/2.0f;
@@ -401,7 +403,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	std::map<int, cv::Mat> emptyLocalMaps;
 	std::map<int, cv::Mat> occupiedLocalMaps;
 
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes.
 	bool graphChanged = false;
 	std::map<int, Transform> transforms;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -448,75 +450,78 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	if(graphChanged && !map_.empty())
 	{
 		UINFO("Graph changed!");
-
-		// 1) recreate all local maps
-		UASSERT(map_.cols == mapInfo_.cols &&
-				map_.rows == mapInfo_.rows);
-		std::map<int, std::pair<int, int> > tmpIndices;
-		for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
+		if(!fullUpdate_)
 		{
-			if(iter->second.first)
+			// 1) recreate all local maps
+			UASSERT(map_.cols == mapInfo_.cols &&
+					map_.rows == mapInfo_.rows);
+			std::map<int, std::pair<int, int> > tmpIndices;
+			for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
 			{
-				emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
-			}
-			if(iter->second.second)
-			{
-				occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
-			}
-			tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
-		}
-		for(int y=1; y<map_.rows-1; ++y)
-		{
-			for(int x=1; x<map_.cols-1; ++x)
-			{
-				float * info = mapInfo_.ptr<float>(y,x);
-				int nodeId = (int)info[0];
-				if(nodeId > 0 && map_.at<char>(y,x) >= 0)
+				if(iter->second.first)
 				{
-					std::map<int, Transform>::iterator tter = transforms.find(nodeId);
-					if(tter != transforms.end() && !uContains(cache_, nodeId))
+					emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
+				}
+				if(iter->second.second)
+				{
+					occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
+				}
+				tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
+			}
+			for(int y=1; y<map_.rows-1; ++y)
+			{
+				for(int x=1; x<map_.cols-1; ++x)
+				{
+					float * info = mapInfo_.ptr<float>(y,x);
+					int nodeId = (int)info[0];
+					if(nodeId > 0 && map_.at<char>(y,x) >= 0)
 					{
-						cv::Point3f pt(info[1], info[2], 0.0f);
-						pt = util3d::transformPoint(pt, tter->second);
-
-						if(minX > pt.x)
-							minX = pt.x;
-						else if(maxX < pt.x)
-							maxX = pt.x;
-
-						if(minY > pt.y)
-							minY = pt.y;
-						else if(maxY < pt.y)
-							maxY = pt.y;
-
-						std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
-						if(map_.at<char>(y, x) == 0)
+						std::map<int, Transform>::iterator tter = transforms.find(nodeId);
+						if(tter != transforms.end() && !uContains(cache_, nodeId))
 						{
-							// ground
-							std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
-							UASSERT(iter != emptyLocalMaps.end());
-							UASSERT(jter->second.first < iter->second.cols);
-							float * ptf = iter->second.ptr<float>(0,jter->second.first++);
-							ptf[0] = pt.x;
-							ptf[1] = pt.y;
-						}
-						else
-						{
-							// obstacle
-							std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
-							UASSERT(iter != occupiedLocalMaps.end());
-							UASSERT(iter!=occupiedLocalMaps.end());
-							UASSERT(jter->second.second < iter->second.cols);
-							float * ptf = iter->second.ptr<float>(0,jter->second.second++);
-							ptf[0] = pt.x;
-							ptf[1] = pt.y;
+							cv::Point3f pt(info[1], info[2], 0.0f);
+							pt = util3d::transformPoint(pt, tter->second);
+
+							if(minX > pt.x)
+								minX = pt.x;
+							else if(maxX < pt.x)
+								maxX = pt.x;
+
+							if(minY > pt.y)
+								minY = pt.y;
+							else if(maxY < pt.y)
+								maxY = pt.y;
+
+							std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
+							if(map_.at<char>(y, x) == 0)
+							{
+								// ground
+								std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
+								UASSERT(iter != emptyLocalMaps.end());
+								UASSERT(jter->second.first < iter->second.cols);
+								float * ptf = iter->second.ptr<float>(0,jter->second.first++);
+								ptf[0] = pt.x;
+								ptf[1] = pt.y;
+							}
+							else
+							{
+								// obstacle
+								std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
+								UASSERT(iter != occupiedLocalMaps.end());
+								UASSERT(iter!=occupiedLocalMaps.end());
+								UASSERT(jter->second.second < iter->second.cols);
+								float * ptf = iter->second.ptr<float>(0,jter->second.second++);
+								ptf[0] = pt.x;
+								ptf[1] = pt.y;
+							}
 						}
 					}
 				}
 			}
+
+			UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
 		}
 
-		UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
 		addedNodes_.clear();
 		map_ = cv::Mat();
 		mapInfo_ = cv::Mat();
@@ -527,25 +532,36 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	else if(!map_.empty())
 	{
 		// update
-		minX=xMin_+margin;
-		minY=yMin_+margin;
+		minX=xMin_+margin+cellSize_/2.0f;
+		minY=yMin_+margin+cellSize_/2.0f;
 		maxX=xMin_+float(map_.cols)*cellSize_ - margin;
 		maxY=yMin_+float(map_.rows)*cellSize_ - margin;
 		undefinedSize = false;
 	}
 
+	bool incrementalGraphUpdate = graphChanged && !fullUpdate_;
+
 	std::list<std::pair<int, Transform> > poses;
-	// place negative poses at the end
-	for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
+	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
+	UDEBUG("Last id = %d", lastId);
+	if(lastId >= 0)
 	{
-		if(iter->first>0)
-		{
-			poses.push_front(*iter);
-		}
-		else
+		for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
 		{
 			poses.push_back(*iter);
 		}
+		// insert negative after
+		for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
+		{
+			if(iter->first < 0)
+			{
+				poses.push_back(*iter);
+			}
+			else
+			{
+				break;
+			}
+		}
 	}
 	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
@@ -662,9 +678,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	{
 		//Get map size
 		float xMin = minX-margin;
+		xMin -= cellSize_/2.0f;
 		float yMin = minY-margin;
+		yMin -= cellSize_/2.0f;
 		float xMax = maxX+margin;
 		float yMax = maxY+margin;
+
 		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
 		   fabs((xMax - xMin) / cellSize_) > 99999)
 		{
@@ -675,7 +694,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 		else
 		{
 			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
-			cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
+			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
 			if(map_.empty())
 			{
 				UDEBUG("Map empty!");
@@ -695,6 +714,11 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 				}
 				else
 				{
+					UASSERT(xMin <= xMin_);
+					UASSERT(yMin <= yMin_);
+					UASSERT(xMax >= xMin_+float(map_.cols)*cellSize_);
+					UASSERT(yMax >= yMin_+float(map_.rows)*cellSize_);
+
 					UDEBUG("Copy map");
 					// copy the old map in the new map
 					// make sure the translation is cellSize
@@ -711,8 +735,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 						yMin = yMin_-float(deltaY)*cellSize_;
 					}
 					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
-					newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
-					newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
+					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
+					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
+					UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
+					UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
 					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
 					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
 					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -743,12 +769,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 					for(int i=0; i<iter->second.cols; ++i)
 					{
 						float * ptf = iter->second.ptr<float>(0,i);
-						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
 						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
 								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
 						char & value = map.at<char>(pt.y, pt.x);
-						if(value != -2)
+						if(value != -2 && (!incrementalGraphUpdate || value==-1))
 						{
 							float * info = mapInfo.ptr<float>(pt.y, pt.x);
 							int nodeId = (int)info[0];
@@ -792,8 +818,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 				if(footprintRadius >= cellSize_*1.5f)
 				{
 					// place free space under the footprint of the robot
-					cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
-					cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
+					cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_, (kter->second.y()-footprintRadius-yMin)/cellSize_);
+					cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_, (kter->second.y()+footprintRadius-yMin)/cellSize_);
 					if(ptBegin.x < 0)
 						ptBegin.x = 0;
 					if(ptEnd.x >= map.cols)
@@ -839,8 +865,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 							if(kter->first > 0)
 							{
 								info[0] = (float)kter->first;
-								info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
-								info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
+								info[1] = float(i) * cellSize_ + xMin;
+								info[2] = float(j) * cellSize_ + yMin;
 								cter->second.first+=1;
 							}
 							value = -2; // free space (footprint)
@@ -853,7 +879,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 					for(int i=0; i<jter->second.cols; ++i)
 					{
 						float * ptf = jter->second.ptr<float>(0,i);
-						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
 						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
 									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
@@ -900,36 +926,110 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 				}
 			}
 
-			// fill holes and put footprint values to empty (0)
-			//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-			//cloud->resize(map.rows*map.cols);
-			//int oi=0;
-			for(int i=1; i<map.rows-1; ++i)
+			if(footprintRadius >= cellSize_*1.5f || incrementalGraphUpdate)
 			{
-				for(int j=1; j<map.cols-1; ++j)
+				for(int i=1; i<map.rows-1; ++i)
 				{
-					char & value = map.at<char>(i, j);
-					if(value == -2)
+					for(int j=1; j<map.cols-1; ++j)
 					{
-						value = 0;
-					}
+						char & value = map.at<char>(i, j);
+						if(value == -2)
+						{
+							value = 0;
+						}
 
-					char sum =  (map.at<char>(i+1, j) != -1?1:0) +
-								(map.at<char>(i-1, j) != -1?1:0) +
-								(map.at<char>(i, j+1) != -1?1:0) +
-								(map.at<char>(i, j-1) != -1?1:0);
-					if(value == -1 && sum >=3)
-					{
-						value = 0;
-					}
+						if(incrementalGraphUpdate && value == -1)
+						{
+							float * info = mapInfo.ptr<float>(i, j);
 
-					//float * info = mapInfo.ptr<float>(i,j);
-					//if(info[0] > 0)
-					//{
-					//	cloud->at(oi).x = info[1];
-					//	cloud->at(oi).y = info[2];
-					//	oi++;
-					//}
+							// fill obstacle
+							if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
+							{
+								value = 100;
+								// associate with the nearest pose
+								if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i+1, j)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+								else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i-1, j)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+							}
+							else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
+							{
+								value = 100;
+								// associate with the nearest pose
+								if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i, j+1)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+								else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i, j-1)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+							}
+							else
+							{
+								// fill empty
+								char sum =  (map.at<char>(i+1, j) == 0?1:0) +
+											(map.at<char>(i-1, j) == 0?1:0) +
+											(map.at<char>(i, j+1) == 0?1:0) +
+											(map.at<char>(i, j-1) == 0?1:0);
+								if(sum >=3)
+								{
+									value = 0;
+									// associate with the nearest pose, only check two cases (as 3 are required)
+									if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
+									{
+										info[0] = mapInfo.ptr<float>(i+1, j)[0];
+										info[1] = float(j) * cellSize_ + xMin;
+										info[2] = float(i) * cellSize_ + yMin;
+										std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+										UASSERT(cter!=cellCount_.end());
+										cter->second.first+=1;
+									}
+									else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
+									{
+										info[0] = mapInfo.ptr<float>(i-1, j)[0];
+										info[1] = float(j) * cellSize_ + xMin;
+										info[2] = float(i) * cellSize_ + yMin;
+										std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+										UASSERT(cter!=cellCount_.end());
+										cter->second.first+=1;
+									}
+								}
+							}
+						}
+
+						//float * info = mapInfo.ptr<float>(i,j);
+						//if(info[0] > 0)
+						//{
+						//	cloud->at(oi).x = info[1];
+						//	cloud->at(oi).y = info[2];
+						//	oi++;
+						//}
+					}
 				}
 			}
 			//if(graphChanged)
@@ -960,7 +1060,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 		}
 	}
 
-	cache_.clear();
+	if(!fullUpdate_)
+	{
+		cache_.clear();
+	}
 
 	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
 }

corelib/src/OctoMap.cpp

@@ -35,9 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OctoMap::OctoMap(float voxelSize, float occupancyThr) :
+OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
 		octree_(new octomap::ColorOcTree(voxelSize)),
-		hasColor_(false)
+		hasColor_(false),
+		fullUpdate_(fullUpdate)
 {
 	octree_->setOccupancyThres(occupancyThr);
 	UASSERT(voxelSize>0.0f);
@@ -67,8 +68,8 @@ void OctoMap::addToCache(int nodeId,
 		const pcl::PointXYZ & viewPoint)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-	cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
+	uInsert(cacheClouds_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
 }
 void OctoMap::addToCache(int nodeId,
 		const cv::Mat & ground,
@@ -78,8 +79,8 @@ void OctoMap::addToCache(int nodeId,
 	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
 	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
 	UDEBUG("nodeId=%d", nodeId);
-	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-	cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
 }
 
 void OctoMap::update(const std::map<int, Transform> & poses)
@@ -113,65 +114,77 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	if(graphChanged)
 	{
 		UINFO("Graph changed!");
-		octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
-		std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
-		int copied=0;
-		for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
-				iter!=occupiedCells_.end();
-				++iter)
+		if(fullUpdate_)
 		{
-			std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
-			if(jter != transforms.end())
+			// clear all but keep cache
+			octree_->clear();
+			occupiedCells_.clear();
+			addedNodes_.clear();
+			keyRay_ = octomap::KeyRay();
+			hasColor_ = false;
+		}
+		else
+		{
+			octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
+			std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
+			int copied=0;
+			for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
+					iter!=occupiedCells_.end();
+					++iter)
 			{
-				octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
-				std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
-				UASSERT(pter != addedNodes_.end());
-
-				cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
-				cvPt = util3d::transformPoint(cvPt, jter->second);
-
-				octomap::OcTreeKey key;
-				if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
+				std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
+				if(jter != transforms.end())
 				{
-					octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
-					if(n)
+					octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+					std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
+					UASSERT(pter != addedNodes_.end());
+
+					cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+					cvPt = util3d::transformPoint(cvPt, jter->second);
+
+					octomap::OcTreeKey key;
+					if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
 					{
-						++copied;
-						uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
-						newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+						octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
+						if(n)
+						{
+							++copied;
+							uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
+							newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+						}
+						else
+						{
+							UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						}
 					}
 					else
 					{
-						UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
 					}
 				}
-				else
+				else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
 				{
-					UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+					UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
 				}
 			}
-			else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
-			{
-				UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
-			}
-		}
-		UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
-		delete octree_;
-		octree_ = newOcTree;
-		occupiedCells_ = newOccupiedCells;
+			UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
+			delete octree_;
+			octree_ = newOcTree;
+			occupiedCells_ = newOccupiedCells;
 
-		//update added poses
-		addedNodes_ = updatedAddedNodes;
+			//update added poses
+			addedNodes_ = updatedAddedNodes;
+		}
 	}
 
 	// Original version from A. Hornung:
 	// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
 	//
+	std::list<std::pair<int, Transform> > orderedPoses;
 	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);
 	if(lastId >= 0)
 	{
-		std::list<std::pair<int, Transform> > orderedPoses;
 		for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
 		{
 			orderedPoses.push_back(*iter);
@@ -188,173 +201,176 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				break;
 			}
 		}
+	}
 
-		UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
-		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+	UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
+	for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+	{
+		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
+		std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
+		std::map<int, cv::Point3f>::iterator viewPointIter;
+		cloudIter = cacheClouds_.find(iter->first);
+		occupancyIter = cache_.find(iter->first);
+		viewPointIter = cacheViewPoints_.find(iter->first);
+		if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
 		{
-			std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-			std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
-			std::map<int, cv::Point3f>::iterator viewPointIter;
-			cloudIter = cacheClouds_.find(iter->first);
-			occupancyIter = cache_.find(iter->first);
-			viewPointIter = cacheViewPoints_.find(iter->first);
-			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
+			UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
+
+			UASSERT(viewPointIter != cacheViewPoints_.end());
+			octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
+			sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+
+			octomap::OcTreeKey tmpKey;
+			if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
+					|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
 			{
-				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
-
-				UASSERT(viewPointIter != cacheViewPoints_.end());
-				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
-				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
-
-				octomap::OcTreeKey tmpKey;
-				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
-						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
-				{
-					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
-				}
-
-				// instead of direct scan insertion, compute update to filter ground:
-				octomap::KeySet free_cells, occupied_cells, ground_cells;
-				// insert ground points only as free:
-				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
-				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
-				Eigen::Affine3f t = iter->second.toEigen3f();
-				for (unsigned int i=0; i<maxGroundPts; ++i)
-				{
-					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
-					}
-
-					octomap::point3d point(pt.x, pt.y, pt.z);
-
-					// only clear space (ground points)
-					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
-					{
-						free_cells.insert(keyRay_.begin(), keyRay_.end());
-					}
-					// occupied endpoint
-					octomap::OcTreeKey key;
-					if (octree_->coordToKeyChecked(point, key))
-					{
-						ground_cells.insert(key);
-
-						octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
-						if(n)
-						{
-							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
-							{
-								hasColor_ = true;
-							}
-							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
-							if(iter->first > 0)
-							{
-								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-							}
-							else
-							{
-								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-							}
-						}
-					}
-				}
-				UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
-
-				// all other points: free on ray, occupied on endpoint:
-				unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
-				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
-				for (unsigned int i=0; i<maxObstaclePts; ++i)
-				{
-					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
-					}
-
-					octomap::point3d point(pt.x, pt.y, pt.z);
-
-					// free cells
-					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
-					{
-						free_cells.insert(keyRay_.begin(), keyRay_.end());
-					}
-					// occupied endpoint
-					octomap::OcTreeKey key;
-					if (octree_->coordToKeyChecked(point, key))
-					{
-						occupied_cells.insert(key);
-
-						octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
-						if(n)
-						{
-							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
-							{
-								hasColor_ = true;
-							}
-							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
-							if(iter->first > 0)
-							{
-								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-							}
-							else
-							{
-								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-							}
-						}
-					}
-				}
-				UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
-
-
-				// mark free cells only if not seen occupied in this cloud
-				for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
-				{
-					if (occupied_cells.find(*it) == occupied_cells.end() &&
-						ground_cells.find(*it) == ground_cells.end())
-					{
-						octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
-						if(n)
-						{
-							std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
-							gter = occupiedCells_.find(n);
-							if(gter != occupiedCells_.end() && gter->second.isObstacle_)
-							{
-								occupiedCells_.erase(gter);
-							}
-						}
-					}
-				}
-
-				// compress map
-				//octree_->prune();
-
-				// ignore negative ids as they are temporary clouds
-				if(iter->first > 0)
-				{
-					addedNodes_.insert(*iter);
-				}
-				UDEBUG("%d: end", iter->first);
+				UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 			}
-			else
+
+			// instead of direct scan insertion, compute update to filter ground:
+			octomap::KeySet free_cells, occupied_cells, ground_cells;
+			// insert ground points only as free:
+			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
+			UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
+			Eigen::Affine3f t = iter->second.toEigen3f();
+			for (unsigned int i=0; i<maxGroundPts; ++i)
 			{
-				UDEBUG("Did not find %d in cache", iter->first);
+				pcl::PointXYZRGB pt;
+				if(occupancyIter != cache_.end())
+				{
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+					pt = pcl::transformPoint(pt, t);
+				}
+				else
+				{
+					pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
+				}
+
+				octomap::point3d point(pt.x, pt.y, pt.z);
+
+				// only clear space (ground points)
+				if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+				{
+					free_cells.insert(keyRay_.begin(), keyRay_.end());
+				}
+				// occupied endpoint
+				octomap::OcTreeKey key;
+				if (octree_->coordToKeyChecked(point, key))
+				{
+					ground_cells.insert(key);
+
+					octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
+					if(n)
+					{
+						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+						{
+							hasColor_ = true;
+						}
+						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+						if(iter->first > 0)
+						{
+							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+						}
+						else
+						{
+							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+						}
+					}
+				}
 			}
+			UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
+
+			// all other points: free on ray, occupied on endpoint:
+			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
+			UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
+			for (unsigned int i=0; i<maxObstaclePts; ++i)
+			{
+				pcl::PointXYZRGB pt;
+				if(occupancyIter != cache_.end())
+				{
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+					pt = pcl::transformPoint(pt, t);
+				}
+				else
+				{
+					pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
+				}
+
+				octomap::point3d point(pt.x, pt.y, pt.z);
+
+				// free cells
+				if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+				{
+					free_cells.insert(keyRay_.begin(), keyRay_.end());
+				}
+				// occupied endpoint
+				octomap::OcTreeKey key;
+				if (octree_->coordToKeyChecked(point, key))
+				{
+					occupied_cells.insert(key);
+
+					octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
+					if(n)
+					{
+						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+						{
+							hasColor_ = true;
+						}
+						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+						if(iter->first > 0)
+						{
+							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+						}
+						else
+						{
+							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+						}
+					}
+				}
+			}
+			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
+
+
+			// mark free cells only if not seen occupied in this cloud
+			for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
+			{
+				if (occupied_cells.find(*it) == occupied_cells.end() &&
+					ground_cells.find(*it) == ground_cells.end())
+				{
+					octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
+					if(n)
+					{
+						std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
+						gter = occupiedCells_.find(n);
+						if(gter != occupiedCells_.end() && gter->second.isObstacle_)
+						{
+							occupiedCells_.erase(gter);
+						}
+					}
+				}
+			}
+
+			// compress map
+			//octree_->prune();
+
+			// ignore negative ids as they are temporary clouds
+			if(iter->first > 0)
+			{
+				addedNodes_.insert(*iter);
+			}
+			UDEBUG("%d: end", iter->first);
+		}
+		else
+		{
+			UDEBUG("Did not find %d in cache", iter->first);
 		}
 	}
-	cache_.clear();
-	cacheClouds_.clear();
-	cacheViewPoints_.clear();
+	if(!fullUpdate_)
+	{
+		cache_.clear();
+		cacheClouds_.clear();
+		cacheViewPoints_.clear();
+	}
 }
 
 void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -494,9 +510,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	return cloud;
 }
 
-cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize)
+cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
 {
-	gridCellSize = octree_->getResolution();
+	UASSERT(treeDepth <= octree_->getTreeDepth());
+	if(treeDepth == 0)
+	{
+		treeDepth = octree_->getTreeDepth();
+	}
+
+	gridCellSize = octree_->getNodeSize(treeDepth);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -505,16 +527,15 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	obstacles->resize(octree_->size());
 	int gi=0;
 	int oi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
+	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
+		octomap::point3d pt = octree_->keyToCoord(it.getKey());
 		if(octree_->isNodeOccupied(*it))
 		{
-			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
 		}
 		else
 		{
-			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
 		}
 	}
@@ -531,17 +552,19 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	}
 
 	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
+	cv::Vec2f * ptr = obstaclesMat.ptr<cv::Vec2f>(0,0);
 	for(unsigned int i=0;i<obstacles->size(); ++i)
 	{
-		obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
-		obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
+		ptr[i][0] = obstacles->at(i).x;
+		ptr[i][1] = obstacles->at(i).y;
 	}
 
 	cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
+	ptr = groundMat.ptr<cv::Vec2f>(0,0);
 	for(unsigned int i=0;i<ground->size(); ++i)
 	{
-		groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;
-		groundMat.at<cv::Vec2f>(i)[1] = ground->at(i).y;
+		ptr[i][0] = ground->at(i).x;
+		ptr[i][1] = ground->at(i).y;
 	}
 
 	std::map<int, Transform> poses;

corelib/src/util3d_mapping.cpp

@@ -877,6 +877,35 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	return map8U;
 }
 
+cv::Mat erodeMap(const cv::Mat & map)
+{
+	UASSERT(map.type() == CV_8SC1);
+	cv::Mat erodedMap = map.clone();
+	for(int i=0; i<map.rows; ++i)
+	{
+		for(int j=0; j<map.cols; ++j)
+		{
+			if(map.at<char>(i, j) == 100)
+			{
+				// remove obstacles which touch at least 3 empty cells but not unknown cells
+				int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
+					(map.at<char>(i-1, j) == 0?1:0) +
+					(map.at<char>(i, j+1) == 0?1:0) +
+					(map.at<char>(i, j-1) == 0?1:0);
+
+				if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
+					map.at<char>(i-1, j) != -1 &&
+					map.at<char>(i, j+1) != -1 &&
+					map.at<char>(i, j-1) != -1)
+				{
+					erodedMap.at<char>(i, j) = 0; // empty
+				}
+			}
+		}
+	}
+	return erodedMap;
+}
+
 }
 
 }