diff --git a/homer_map_manager/CMakeLists.txt b/homer_map_manager/CMakeLists.txt
index c659d0ab71d24a20af827740223eeada98e16713..ce519e8fbfeeab7b3b28762b32c0a102624597fc 100644
--- a/homer_map_manager/CMakeLists.txt
+++ b/homer_map_manager/CMakeLists.txt
@@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer
 
 find_package( Eigen3 REQUIRED )
 
+set(CMAKE_BUILD_TYPE Release)
+
 find_package(OpenMP)
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
index 1947da82f3a60d2e3b31b6d03e0c62c646f28656..a7b3c7c98afa23f4b9271d81e5e55277d14d9991 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
@@ -23,7 +23,7 @@ class MaskingManager {
     /** @brief The destructor. */
     virtual ~MaskingManager();
 
-    void updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
+    nav_msgs::OccupancyGrid::ConstPtr updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
 
     /** modifies either the masking layer or the slam layer (accordingly to the
      * given map layer in the msg */
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index 8e3e5fe2ca2202072d29c2faccd9f5f55bf3101a..6368ff308a455808f137071db5a2c37935cc1f99 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -52,12 +52,6 @@ MapManager::~MapManager()
 
 void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
 {
-    //TODO possible crash
-    if(m_MapLayers[type]->info.width != layer->info.width)
-    {
-        
-        ROS_INFO_STREAM("Map_manager: map size changed!");
-    }
     m_MapLayers[type] 	= layer;
     if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
     {
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index 4ee2ff60823e43a3abb3bb3bdf7e040225238b68..2ee1dd73257851f42fad2d8cd7a7b211dc0e4c7f 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/homer_map_manager/src/Managers/MaskingManager.cpp
@@ -17,16 +17,17 @@ MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
 
 MaskingManager::~MaskingManager() {}
 
-void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
-    if (m_SlamMap.info.width != mapInfo.width) {
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
+    if (m_SlamMap.info.width < mapInfo.width || m_SlamMap.info.height < mapInfo.height) {
         m_SlamMap.info = mapInfo;
 
         int x_add_left =
-            (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x) /
+            (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x  + 0.025) /
             mapInfo.resolution;
         int y_add_up =
-            (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y) /
+            (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y  + 0.025) /
             mapInfo.resolution;
+        ROS_INFO_STREAM("x add "<<x_add_left<<" y add "<<y_add_up);
 
         std::vector<signed char> tmpData = m_MaskingMap.data;
 
@@ -43,11 +44,13 @@ void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
         }
         m_MaskingMap.info = mapInfo;
     }
+        return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
 }
 
 nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
     homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
     // reset SLAM mask map before each masking
+    m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width);
     std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
               homer_mapnav_msgs::ModifyMap::NOT_MASKED);
     drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 1060e1a32be2b3c084c417fde7bb0504bd5f30ea..185dce9f41d63066ad349dc532d4319fbf3cabc1 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -132,8 +132,9 @@ MapManagerNode::~MapManagerNode() {
 
 void MapManagerNode::callbackSLAMMap(
     const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->updateMapInfo(msg->info);
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
     m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
-    m_MaskingManager->updateMapInfo(msg->info);
 }
 
 void MapManagerNode::callbackRapidMap(
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 464a914b1b610efe866d3a78712066e40df9b706..a1f8b468ec61961af090bf0a0c485633975696d5 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -818,15 +818,15 @@ void OccupancyMap::applyChanges()
       if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
       {
 		
-		if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
-				m_MeasurementCount[x + m_metaData.width * (y+1)]++;
-		if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
-				m_MeasurementCount[x + m_metaData.width * (y-1)]++;
-		if(m_MeasurementCount[i-1] > 1)
-			m_MeasurementCount[i-1]++;
-		if(m_MeasurementCount[i+1] > 1)
-			m_MeasurementCount[i+1]++;
-        m_OccupancyCount[i] += 4;
+		//if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
+				//m_MeasurementCount[x + m_metaData.width * (y+1)]++;
+		//if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
+				//m_MeasurementCount[x + m_metaData.width * (y-1)]++;
+		//if(m_MeasurementCount[i-1] > 1)
+			//m_MeasurementCount[i-1]++;
+		//if(m_MeasurementCount[i+1] > 1)
+			//m_MeasurementCount[i+1]++;
+        m_OccupancyCount[i] ++;
       }
     }
   }