diff --git a/CMakeLists.txt b/CMakeLists.txt
index fa5fa7f1..6742533f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,10 +18,13 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.16.3 REQUIRED)
+find_package(RTABMap 0.17.0 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
+find_package(PCL 1.7 REQUIRED)
+add_definitions(${PCL_DEFINITIONS}) # To include -march=native if set
+
 #Qt stuff
 # If librtabmap_gui.so is found, rtabmapviz will be built
 # If rviz is found, plugins will be built
diff --git a/include/rtabmap_ros/MapsManager.h b/include/rtabmap_ros/MapsManager.h
index 2f8424ba..46e85e21 100644
--- a/include/rtabmap_ros/MapsManager.h
+++ b/include/rtabmap_ros/MapsManager.h
@@ -52,6 +52,7 @@ public:
 	bool hasSubscribers() const;
 	void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const;
 	void setParameters(const rtabmap::ParametersMap & parameters);
+	void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses);
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
@@ -69,7 +70,6 @@ public:
 			const std::string & mapFrameId);
 
 	cv::Mat getGridMap(
-			const std::map<int, rtabmap::Transform> & filteredPoses,
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
diff --git a/package.xml b/package.xml
index 465f6d6e..8a33602e 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.16.3</version>
+  <version>0.17.0</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 6706363f..62810b10 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -346,6 +346,13 @@ void CoreWrapper::onInit()
 				Parameters::kGridFromDepth().c_str());
 		parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
 	}
+	if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end())
+	{
+		NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is true.",
+				Parameters::kGridRangeMax().c_str(),
+				Parameters::defaultGridRangeMax());
+		parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
+	}
 	int regStrategy = Parameters::defaultRegStrategy();
 	Parameters::parse(parameters_, Parameters::kRegStrategy(), regStrategy);
 	if(subscribeScan2d &&
@@ -465,6 +472,16 @@ void CoreWrapper::onInit()
 	// Init RTAB-Map
 	rtabmap_.init(parameters_, databasePath_);
 
+	if(rtabmap_.getMemory())
+	{
+		float xMin, yMin, gridCellSize;
+		cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
+		if(!map.empty())
+		{
+			mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses());
+		}
+	}
+
 	if(databasePath_.size() && rtabmap_.getMemory())
 	{
 		NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
@@ -623,6 +640,17 @@ CoreWrapper::~CoreWrapper()
 	nh.deleteParam("is_rtabmap_paused");
 
 	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
+	if(rtabmap_.getMemory())
+	{
+		// save the grid map
+		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
+		cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
+		if(!pixels.empty())
+		{
+			rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
+		}
+	}
+
 	rtabmap_.close();
 	printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
 }
@@ -2031,57 +2059,33 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::
 
 bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
 {
-	std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
-	if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
+	// create the grid map
+	float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
+	cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
+
+	if(!pixels.empty())
 	{
-		std::map<int, Transform> nearestPoses;
-		std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
-		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
-		{
-			std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
-			if(pter != filteredPoses.end())
-			{
-				nearestPoses.insert(*pter);
-			}
-		}
-		filteredPoses = nearestPoses;
-	}
+		//init
+		res.map.info.resolution = gridCellSize;
+		res.map.info.origin.position.x = 0.0;
+		res.map.info.origin.position.y = 0.0;
+		res.map.info.origin.position.z = 0.0;
+		res.map.info.origin.orientation.x = 0.0;
+		res.map.info.origin.orientation.y = 0.0;
+		res.map.info.origin.orientation.z = 0.0;
+		res.map.info.origin.orientation.w = 1.0;
 
-	filteredPoses = mapsManager_.updateMapCaches(
-			filteredPoses,
-			rtabmap_.getMemory(),
-			true,
-			false);
-	if(filteredPoses.size())
-	{
-		// create the grid map
-		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = mapsManager_.getGridMap(filteredPoses, xMin, yMin, gridCellSize);
+		res.map.info.width = pixels.cols;
+		res.map.info.height = pixels.rows;
+		res.map.info.origin.position.x = xMin;
+		res.map.info.origin.position.y = yMin;
+		res.map.data.resize(res.map.info.width * res.map.info.height);
 
-		if(!pixels.empty())
-		{
-			//init
-			res.map.info.resolution = gridCellSize;
-			res.map.info.origin.position.x = 0.0;
-			res.map.info.origin.position.y = 0.0;
-			res.map.info.origin.position.z = 0.0;
-			res.map.info.origin.orientation.x = 0.0;
-			res.map.info.origin.orientation.y = 0.0;
-			res.map.info.origin.orientation.z = 0.0;
-			res.map.info.origin.orientation.w = 1.0;
+		memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
 
-			res.map.info.width = pixels.cols;
-			res.map.info.height = pixels.rows;
-			res.map.info.origin.position.x = xMin;
-			res.map.info.origin.position.y = yMin;
-			res.map.data.resize(res.map.info.width * res.map.info.height);
-
-			memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
-
-			res.map.header.frame_id = mapFrameId_;
-			res.map.header.stamp = ros::Time::now();
-			return true;
-		}
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+		return true;
 	}
 	return false;
 }
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index 93fdaaf5..2368760e 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -290,6 +290,11 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
 #endif
 }
 
+void MapsManager::set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses)
+{
+	occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
+}
+
 void MapsManager::clear()
 {
 	gridMaps_.clear();
@@ -1195,7 +1200,7 @@ void MapsManager::publishMaps(
 
 		// create the grid map
 		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = this->getGridMap(poses, xMin, yMin, gridCellSize);
+		cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
 
 		if(!pixels.empty())
 		{
@@ -1244,7 +1249,6 @@ void MapsManager::publishMaps(
 }
 
 cv::Mat MapsManager::getGridMap(
-		const std::map<int, rtabmap::Transform> & poses,
 		float & xMin,
 		float & yMin,
 		float & gridCellSize)