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/MapManager.h b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
index 3d82a8820a0a295e521b83ac9bf8c96deeb0606f..d5a74ce4be03bccc050bb037035a18b37e859814 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
@@ -60,12 +60,6 @@ class MapManager
      * @brief clearMapLayers Clear all map layers
      */
     void clearMapLayers();
-
-    /** getters */
-    double getHeight() { return m_Height; }
-    double getWidth() { return m_Width; }
-    double getResolution() { return m_Resolution; }
-    geometry_msgs::Pose getOrigin() { return m_Origin; }
     
     void updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr& msg );
 
@@ -94,11 +88,7 @@ class MapManager
     std::map<int, bool> m_MapVisibility;
 
     //sizes of the last slam map
-    double m_Height;
-    double m_Width;
-    double m_Resolution;
     bool m_got_transform;
-    geometry_msgs::Pose m_Origin;
     geometry_msgs::PoseStamped::ConstPtr m_pose;
 	tf::StampedTransform m_sick_transform;
 	tf::StampedTransform m_hokuyo_transform;
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 a372cf639d0c18d0a1125f750008f9521a0fae73..a7b3c7c98afa23f4b9271d81e5e55277d14d9991 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
@@ -3,28 +3,32 @@
 
 #include <ros/ros.h>
 
-#include <nav_msgs/OccupancyGrid.h>
 #include <homer_mapnav_msgs/ModifyMap.h>
+#include <nav_msgs/MapMetaData.h>
+#include <nav_msgs/OccupancyGrid.h>
 
 #include <sstream>
 
 /**
  * @class  MaskingManager
- * @brief  Manages a map that can overwrite values in the SLAM map or store it in a separate layer
+ * @brief  Manages a map that can overwrite values in the SLAM map or store it
+ * in a separate layer
  * @author Malte Knauf, David Gossow
  */
-class MaskingManager
-{
-  public:
-
+class MaskingManager {
+   public:
     /** @brief The constructor. */
-    MaskingManager(int mapSize, float resolution);
+    MaskingManager(nav_msgs::MapMetaData mapInfo);
 
     /** @brief The destructor. */
     virtual ~MaskingManager();
 
-    /** modifies either the masking layer or the slam layer (accordingly to the given map layer in the msg */
-    nav_msgs::OccupancyGrid::ConstPtr modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg);
+    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 */
+    nav_msgs::OccupancyGrid::ConstPtr modifyMap(
+        homer_mapnav_msgs::ModifyMap::ConstPtr msg);
 
     /** resets the masking map layer */
     nav_msgs::OccupancyGrid::ConstPtr resetMap();
@@ -32,22 +36,18 @@ class MaskingManager
     /** replaces the masking map layer */
     void replaceMap(nav_msgs::OccupancyGrid map);
 
-  private:
-
+   private:
     /** stores the masking values in the dedicated masking map */
     nav_msgs::OccupancyGrid m_MaskingMap;
     /** stores the masking values that are afterwards sent to the slam map */
     nav_msgs::OccupancyGrid m_SlamMap;
 
-    /** sizes of the masking map layer */
-    int m_Width, m_Height;
-    float m_CellSize;
-
     /** tools to draw masking polygons */
-    void drawPolygon ( std::vector< geometry_msgs::Point > vertices, int value, int mapLayer );
-    void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
-    void fillPolygon ( std::vector<int> &data, int x, int y, int value );
+    void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
+                     int mapLayer);
+    void drawLine(std::vector<int> &data, int startX, int startY, int endX,
+                  int endY, int value);
+    void fillPolygon(std::vector<int> &data, int x, int y, int value);
 };
 
 #endif
-
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index b6bbf506e1fbe29e74a707bd0b10e05269e4ac41..6368ff308a455808f137071db5a2c37935cc1f99 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -23,10 +23,6 @@ MapManager::MapManager(ros::NodeHandle* nh)
     {
     	m_MapVisibility[m_laser_layers[i]] = true;
     }
-
-    m_Height 			= -1;
-    m_Width 			= -1;
-    m_Resolution 		= -1;
     
 	try
 	{
@@ -57,13 +53,8 @@ MapManager::~MapManager()
 void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
 {
     m_MapLayers[type] 	= layer;
-    //if slam map update map sizes
     if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
     {
-        m_Height 		= layer->info.height;
-        m_Width 		= layer->info.width;
-        m_Resolution 	= layer->info.resolution;
-        m_Origin 		= layer->info.origin;
  	    sendMergedMap();
     }
 }
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index 1b3af14db71a07f09e56a1b2449f32e0540bf32b..2ee1dd73257851f42fad2d8cd7a7b211dc0e4c7f 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/homer_map_manager/src/Managers/MaskingManager.cpp
@@ -2,184 +2,184 @@
 
 using namespace std;
 
-MaskingManager::MaskingManager(int mapSize, float resolution)
-{
-    m_CellSize = resolution;
-    m_Width = mapSize / m_CellSize + 1;
-    m_Height = mapSize / m_CellSize + 1;
-    ROS_INFO_STREAM( "Creating " << m_Width << " x " << m_Height << " map." );
-    m_MaskingMap.info.resolution = m_CellSize;
-    m_MaskingMap.info.height = m_Height;
-    m_MaskingMap.info.width = m_Width;
-    m_MaskingMap.info.origin.position.x = -m_Height * resolution / 2;
-    m_MaskingMap.info.origin.position.y = -m_Width  * resolution / 2;
-    m_MaskingMap.data.resize(m_Width * m_Height);
-    std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
-
-    m_SlamMap.info.resolution = m_CellSize;
-    m_SlamMap.info.height = m_Height;
-    m_SlamMap.info.width = m_Width;
-    m_SlamMap.info.origin.position.x = -m_Height * resolution / 2;
-    m_SlamMap.info.origin.position.y = -m_Width  * resolution / 2;
-    m_SlamMap.data.resize(m_Width * m_Height);
-    std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
+    m_MaskingMap.info = mapInfo;
+    m_MaskingMap.data.resize(m_MaskingMap.info.width *
+                             m_MaskingMap.info.height);
+    std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+              homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+    m_SlamMap.info = mapInfo;
+    m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
+    std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+              homer_mapnav_msgs::ModifyMap::NOT_MASKED);
 }
 
-MaskingManager::~MaskingManager()
-{}
+MaskingManager::~MaskingManager() {}
 
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
-{
-    //reset SLAM mask map before each masking
-    std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+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  + 0.025) /
+            mapInfo.resolution;
+        int y_add_up =
+            (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;
+
+        m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
+        std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+                  homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+        for (int x = 0; x < m_MaskingMap.info.width; x++) {
+            for (int y = 0; y < m_MaskingMap.info.height; y++) {
+                int i = y * m_MaskingMap.info.width + x;
+                int j = (y + y_add_up) * mapInfo.width + x + x_add_left;
+                m_MaskingMap.data[j] = tmpData[i];
+            }
+        }
+        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);
 
     nav_msgs::OccupancyGrid::ConstPtr ret;
-    if(msg->mapLayer == 0)
-    {
-        ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_SlamMap);
-    }
-    else
-    {
-        ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
+    if (msg->mapLayer == 0) {
+        ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
+    } else {
+        ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
     }
     return ret;
 }
 
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap()
-{
-    std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
-    nav_msgs::OccupancyGrid::ConstPtr ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
+    std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+              homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+    nav_msgs::OccupancyGrid::ConstPtr ret =
+        boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
     return ret;
 }
 
-void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map)
-{
-    if(map.data.size() != 0)
+void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) {
+    if (map.data.size() != 0)
         m_MaskingMap = map;
     else
-        std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+        std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+                  homer_mapnav_msgs::ModifyMap::NOT_MASKED);
 }
 
-void MaskingManager::drawPolygon ( vector< geometry_msgs::Point > vertices , int value , int mapLayer)
-{
-  if ( vertices.size() == 0 )
-  {
-    ROS_INFO_STREAM( "No vertices given!" );
-    return;
-  }
-    //make temp. map
-    std::vector<int> data(m_Width * m_Height);
-    for ( int i = 0; i < data.size(); i++ )
-    {
-        data[i] = 0;
+void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices,
+                                 int value, int mapLayer) {
+    if (vertices.size() == 0) {
+        ROS_INFO_STREAM("No vertices given!");
+        return;
     }
-
-  //draw the lines surrounding the polygon
-  for ( unsigned int i = 0; i < vertices.size(); i++ )
-  {
-    int i2 = ( i+1 ) % vertices.size();
-    drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y,  1);
-  }
-  //calculate a point in the middle of the polygon
-  float midX = 0;
-  float midY = 0;
-  for ( unsigned int i = 0; i < vertices.size(); i++ )
-  {
-      midX += vertices[i].x;
-      midY += vertices[i].y;
-  }
-  midX /= vertices.size();
-  midY /= vertices.size();
-  //fill polygon
-  fillPolygon ( data, (int)midX, (int)midY, 1 );
-
-    //copy polygon to masking map or slam map (according to parameter mapLayer)
-    for ( int i = 0; i < data.size(); i++ )
-    {
-        if ( data[i] != 0 )
-        {
-            switch(mapLayer)
-            {
-            case 0:         //SLAM map
-                m_SlamMap.data[i] = value;
-                break;
-            case 1:         //Kinect Map. apply masking to masking map
-            case 2:         //masking map
-                m_MaskingMap.data[i] = value;
-                break;
+    // make temp. map
+    std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
+    std::fill(data.begin(), data.end(), 0);
+
+    // draw the lines surrounding the polygon
+    for (unsigned int i = 0; i < vertices.size(); i++) {
+        int i2 = (i + 1) % vertices.size();
+        drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
+                 vertices[i2].y, 1);
+    }
+    // calculate a point in the middle of the polygon
+    float midX = 0;
+    float midY = 0;
+    for (unsigned int i = 0; i < vertices.size(); i++) {
+        midX += vertices[i].x;
+        midY += vertices[i].y;
+    }
+    midX /= vertices.size();
+    midY /= vertices.size();
+    // fill polygon
+    fillPolygon(data, (int)midX, (int)midY, 1);
+
+    // copy polygon to masking map or slam map (according to parameter mapLayer)
+    for (int i = 0; i < data.size(); i++) {
+        if (data[i] != 0) {
+            switch (mapLayer) {
+                case 0:  // SLAM map
+                    m_SlamMap.data[i] = value;
+                    break;
+                case 1:  // Kinect Map. apply masking to masking map
+                case 2:  // masking map
+                    m_MaskingMap.data[i] = value;
+                    break;
             }
         }
     }
 }
 
-void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value )
-{
-  //bresenham algorithm
-  int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
-  // compute distances
-  dx = endX - startX;
-  dy = endY - startY;
-
-  // compute increment
-  if ( dx < 0 )
-  {
-    incx = -1;
-    dx = -dx;
-  }
-  else
-  {
-    incx = dx ? 1 : 0;
-  }
-
-  if ( dy < 0 )
-  {
-    incy = -1;
-    dy = -dy;
-  }
-  else
-  {
-    incy = dy ? 1 : 0;
-  }
-
-  // which distance is greater?
-  dist = ( dx > dy ) ? dx : dy;
-  // initializing
-  x = startX;
-  y = startY;
-  xerr = dx;
-  yerr = dy;
-
-  // compute cells
-  for ( t = 0; t < dist; t++ )
-  {
-    data[x + m_Width * y] = value;
-
-    xerr += dx;
-    yerr += dy;
-    if ( xerr > dist )
-    {
-      xerr -= dist;
-      x += incx;
+void MaskingManager::drawLine(std::vector<int> &data, int startX, int startY,
+                              int endX, int endY, int value) {
+    // bresenham algorithm
+    int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+    // compute distances
+    dx = endX - startX;
+    dy = endY - startY;
+
+    // compute increment
+    if (dx < 0) {
+        incx = -1;
+        dx = -dx;
+    } else {
+        incx = dx ? 1 : 0;
     }
-    if ( yerr > dist )
-    {
-      yerr -= dist;
-      y += incy;
+
+    if (dy < 0) {
+        incy = -1;
+        dy = -dy;
+    } else {
+        incy = dy ? 1 : 0;
     }
-  }
-}
 
+    // which distance is greater?
+    dist = (dx > dy) ? dx : dy;
+    // initializing
+    x = startX;
+    y = startY;
+    xerr = dx;
+    yerr = dy;
+
+    // compute cells
+    for (t = 0; t < dist; t++) {
+        data[x + m_MaskingMap.info.width * y] = value;
+
+        xerr += dx;
+        yerr += dy;
+        if (xerr > dist) {
+            xerr -= dist;
+            x += incx;
+        }
+        if (yerr > dist) {
+            yerr -= dist;
+            y += incy;
+        }
+    }
+}
 
-void MaskingManager::fillPolygon ( std::vector<int> &data, int x, int y, int value )
-{
-  int index = x + m_Width * y;
-  if ( value != data[index] )
-  {
-    data[index] = value;
-    fillPolygon ( data, x + 1, y, value );
-    fillPolygon ( data, x - 1, y, value );
-    fillPolygon ( data, x, y + 1, value );
-    fillPolygon ( data, x, y - 1, value );
-  }
+void MaskingManager::fillPolygon(std::vector<int> &data, int x, int y,
+                                 int value) {
+    int index = x + m_MaskingMap.info.width * y;
+    if (value != data[index]) {
+        data[index] = value;
+        fillPolygon(data, x + 1, y, value);
+        fillPolygon(data, x - 1, y, value);
+        fillPolygon(data, x, y + 1, value);
+        fillPolygon(data, x, y - 1, value);
+    }
 }
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 59bca4741e4c1462c7f7b7592a12736a7c424924..185dce9f41d63066ad349dc532d4319fbf3cabc1 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -1,419 +1,457 @@
 #include <homer_map_manager/map_manager_node.h>
 //#include <gperftools/profiler.h>
 
-MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
-{
+MapManagerNode::MapManagerNode(ros::NodeHandle* nh) {
     m_LastLaserTime = ros::Time::now();
 
     int mapSize;
     float resolution;
-	ros::param::param("/homer_mapping/size", mapSize, (int) 35);
-	ros::param::param("/homer_mapping/resolution", resolution, (float) 0.05);
-    ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool) false);
-    ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time, (float) 0.5);
+    ros::param::param("/homer_mapping/size", mapSize, (int)35);
+    ros::param::param("/homer_mapping/resolution", resolution, (float)0.05);
+    ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool)false);
+    ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
+                      (float)0.5);
     m_lastROIPoll = ros::Time::now();
     m_MapManager = new MapManager(nh);
     m_POIManager = new PoiManager(nh);
     m_ROIManager = new RoiManager(nh);
-    m_MaskingManager = new MaskingManager(mapSize, resolution);
-
-    //subscriptions of MapManagerModule
-    m_RapidMapSubscriber 		= nh->subscribe("/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
-    m_OctomapSubscriber 		= nh->subscribe("/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
-    m_SLAMMapSubscriber 		= nh->subscribe("/homer_mapping/slam_map", 1, &MapManagerNode::callbackSLAMMap, this);
-    m_SaveMapSubscriber 		= nh->subscribe("/map_manager/save_map", 1, &MapManagerNode::callbackSaveMap, this);
-	m_LoadMapSubscriber 		= nh->subscribe("/map_manager/load_map", 1, &MapManagerNode::callbackLoadMap, this);
-    m_MapVisibilitySubscriber 	= nh->subscribe("/map_manager/toggle_map_visibility", 1, &MapManagerNode::callbackMapVisibility, this);
-    m_LaserScanSubscriber 		= nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
-    m_BackLaserScanSubscriber 	= nh->subscribe("/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
-    m_FrontLaserScanSubscriber 	= nh->subscribe("/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
-	m_PoseSubscriber 			= nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
-
-    //subscriptions of PoiManager
-    m_AddPOISubscriber 			= nh->subscribe("/map_manager/add_POI", 20, &MapManagerNode::callbackAddPOI, this);
-    m_ModifyPOISubscriber 		= nh->subscribe("/map_manager/modify_POI", 100, &MapManagerNode::callbackModifyPOI, this);
-    m_DeletePOISubscriber 		= nh->subscribe("/map_manager/delete_POI", 100, &MapManagerNode::callbackDeletePOI, this);
-    m_GetPOIsService 			= nh->advertiseService("/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
-    
-	// Services for Map Handling
-    m_SaveMapService 			= nh->advertiseService("/map_manager/save_map", &MapManagerNode::saveMapService, this);
-    m_ResetMapService 			= nh->advertiseService("/map_manager/reset_map", &MapManagerNode::resetMapService, this);
-    m_LoadMapService 			= nh->advertiseService("/map_manager/load_map", &MapManagerNode::loadMapService, this);
-
-    //subscriptions of RoiManager
-    m_AddROISubscriber 			= nh->subscribe("/map_manager/add_ROI", 20, &MapManagerNode::callbackAddROI, this);
-    m_ModifyROISubscriber 		= nh->subscribe("/map_manager/modify_ROI", 100, &MapManagerNode::callbackModifyROI, this);
-    m_DeleteROIByNameSubscriber = nh->subscribe("/map_manager/delete_ROI_by_name", 100, &MapManagerNode::callbackDeleteROIbyName, this);
-    m_DeleteROIByIDSubscriber 	= nh->subscribe("/map_manager/delete_ROI_by_id", 100, &MapManagerNode::callbackDeleteROIbyID, this);
-    m_GetROIsService 			= nh->advertiseService("/map_manager/get_rois", &MapManagerNode::getROIsService, this);	
-	m_GetROINameService			= nh->advertiseService("/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
-    m_PoiInsideROISService   	= nh->advertiseService("/map_manager/point_inside_rois", &MapManagerNode::pointInsideRoisService, this);
-    if(m_roi_polling)
-    {
-    	m_RoiPollPublisher		= nh->advertise<homer_mapnav_msgs::RoiChange>("/map_manager/roi_change", 1);
-	}
-
-    //subscribtions of MaskingMapModule
-    m_ModifyMapSubscriber 		= nh->subscribe("/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
-    m_ResetMapsSubscriber 		= nh->subscribe("/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
-
-    //loaded map publisher
-    m_LoadedMapPublisher 		= nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
-
-    //mask slam publisher
-    m_MaskSlamMapPublisher 		= nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
-
-
-    //load map file from config if present
+
+    nav_msgs::MapMetaData mapInfo;
+    mapInfo.width = mapSize / resolution;
+    mapInfo.height = mapSize / resolution;
+    mapInfo.resolution = resolution;
+    mapInfo.origin.position.x = 0;
+    mapInfo.origin.position.y = 0;
+    mapInfo.origin.position.z = 0;
+    mapInfo.origin.orientation.x = 0;
+    mapInfo.origin.orientation.y = 0;
+    mapInfo.origin.orientation.z = 0;
+    mapInfo.origin.orientation.w = 1;
+
+    m_MaskingManager = new MaskingManager(mapInfo);
+
+    // subscriptions of MapManagerModule
+    m_RapidMapSubscriber = nh->subscribe(
+        "/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
+    m_OctomapSubscriber = nh->subscribe(
+        "/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
+    m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1,
+                                        &MapManagerNode::callbackSLAMMap, this);
+    m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1,
+                                        &MapManagerNode::callbackSaveMap, this);
+    m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1,
+                                        &MapManagerNode::callbackLoadMap, this);
+    m_MapVisibilitySubscriber =
+        nh->subscribe("/map_manager/toggle_map_visibility", 1,
+                      &MapManagerNode::callbackMapVisibility, this);
+    m_LaserScanSubscriber =
+        nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
+    m_BackLaserScanSubscriber = nh->subscribe(
+        "/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
+    m_FrontLaserScanSubscriber = nh->subscribe(
+        "/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
+    m_PoseSubscriber =
+        nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
+
+    // subscriptions of PoiManager
+    m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20,
+                                       &MapManagerNode::callbackAddPOI, this);
+    m_ModifyPOISubscriber =
+        nh->subscribe("/map_manager/modify_POI", 100,
+                      &MapManagerNode::callbackModifyPOI, this);
+    m_DeletePOISubscriber =
+        nh->subscribe("/map_manager/delete_POI", 100,
+                      &MapManagerNode::callbackDeletePOI, this);
+    m_GetPOIsService = nh->advertiseService(
+        "/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
+
+    // Services for Map Handling
+    m_SaveMapService = nh->advertiseService(
+        "/map_manager/save_map", &MapManagerNode::saveMapService, this);
+    m_ResetMapService = nh->advertiseService(
+        "/map_manager/reset_map", &MapManagerNode::resetMapService, this);
+    m_LoadMapService = nh->advertiseService(
+        "/map_manager/load_map", &MapManagerNode::loadMapService, this);
+
+    // subscriptions of RoiManager
+    m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20,
+                                       &MapManagerNode::callbackAddROI, this);
+    m_ModifyROISubscriber =
+        nh->subscribe("/map_manager/modify_ROI", 100,
+                      &MapManagerNode::callbackModifyROI, this);
+    m_DeleteROIByNameSubscriber =
+        nh->subscribe("/map_manager/delete_ROI_by_name", 100,
+                      &MapManagerNode::callbackDeleteROIbyName, this);
+    m_DeleteROIByIDSubscriber =
+        nh->subscribe("/map_manager/delete_ROI_by_id", 100,
+                      &MapManagerNode::callbackDeleteROIbyID, this);
+    m_GetROIsService = nh->advertiseService(
+        "/map_manager/get_rois", &MapManagerNode::getROIsService, this);
+    m_GetROINameService = nh->advertiseService(
+        "/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
+    m_PoiInsideROISService =
+        nh->advertiseService("/map_manager/point_inside_rois",
+                             &MapManagerNode::pointInsideRoisService, this);
+    if (m_roi_polling) {
+        m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>(
+            "/map_manager/roi_change", 1);
+    }
+
+    // subscribtions of MaskingMapModule
+    m_ModifyMapSubscriber = nh->subscribe(
+        "/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
+    m_ResetMapsSubscriber = nh->subscribe(
+        "/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
+
+    // loaded map publisher
+    m_LoadedMapPublisher =
+        nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
+
+    // mask slam publisher
+    m_MaskSlamMapPublisher =
+        nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
+
+    // load map file from config if present
     std::string mapfile;
-	ros::param::get("/map_manager/default_mapfile", mapfile);
-    if(mapfile != "")
-    {
-		std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
-        mapfileMsg->data= mapfile;
-        callbackLoadMap ( mapfileMsg );
+    ros::param::get("/map_manager/default_mapfile", mapfile);
+    if (mapfile != "") {
+        std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+        mapfileMsg->data = mapfile;
+        callbackLoadMap(mapfileMsg);
     }
-	  m_POIManager->broadcastPoiList();
-	  m_ROIManager->broadcastRoiList();
+    m_POIManager->broadcastPoiList();
+    m_ROIManager->broadcastRoiList();
 }
 
-MapManagerNode::~MapManagerNode()
-{
-    if(m_MapManager) delete m_MapManager;
-    if(m_POIManager) delete m_POIManager;
-    if(m_ROIManager) delete m_ROIManager;
-    if(m_MaskingManager) delete m_MaskingManager;
+MapManagerNode::~MapManagerNode() {
+    if (m_MapManager) delete m_MapManager;
+    if (m_POIManager) delete m_POIManager;
+    if (m_ROIManager) delete m_ROIManager;
+    if (m_MaskingManager) delete m_MaskingManager;
 }
 
-void MapManagerNode::callbackSLAMMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
+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);
 }
 
-void MapManagerNode::callbackRapidMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
-	m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
+void MapManagerNode::callbackRapidMap(
+    const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
+}
+
+void MapManagerNode::callbackOctomapMap(
+    const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
+                                 msg);
+
+    //	nav_msgs::OccupancyGrid mergedMap;
+    //
+    //   	int width = 701;
+    //    int height = 701;
+    //    float resolution = 0.05;
+    //
+    //    int ByteSize = width * height;
+    //
+    //    mergedMap.header.stamp = ros::Time::now();
+    //    mergedMap.header.frame_id = "map";
+    //    mergedMap.info.width = width;
+    //    mergedMap.info.height = height;
+    //    mergedMap.info.resolution = resolution;
+    //    mergedMap.info.origin.position.x = -height*resolution/2;
+    //    mergedMap.info.origin.position.y = -width*resolution/2;
+    //    mergedMap.info.origin.orientation.w = 1.0;
+    //    mergedMap.info.origin.orientation.x = 0.0;
+    //    mergedMap.info.origin.orientation.y = 0.0;
+    //    mergedMap.info.origin.orientation.z = 0.0;
+    //  	mergedMap.data.resize(ByteSize,-1);
+
+    //	  for ( int y = 0; y < msg->info.height; y++ )
+    //	  {
+    //		for ( int x = 0; x < msg->info.width; x++ )
+    //		{
+    //		  	int i = x + y * msg->info.width;
+
+    //
+    //			//if cell is occupied by kinect obstacle merge cell with merged
+    //map
+    //			if(msg->data[i] ==
+    //homer_mapnav_msgs::ModifyMap::BLOCKED)
+    //			{
+
+    //				Eigen::Vector2i point(x,y);
+    //				geometry_msgs::Point tmp = map_tools::fromMapCoords( point
+    //,msg->info.origin, msg->info.resolution);
+    //				point = map_tools::toMapCoords(tmp , mergedMap.info.origin,
+    //mergedMap.info.resolution);
+    //				int k = point.y() * mergedMap.info.width +
+    //point.x();
+    //				mergedMap.data[k] =
+    //homer_mapnav_msgs::ModifyMap::DEPTH;
+    //			}
+    //		}
+    //	  }
+    //
+    //
+    //	m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
+    //boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
 }
 
-void MapManagerNode::callbackOctomapMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
-	m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, msg);
-
-//	nav_msgs::OccupancyGrid mergedMap;
-//	
-//   	int width = 701;
-//    int height = 701;
-//    float resolution = 0.05;
-//  
-//    int ByteSize = width * height;
-//    
-//    mergedMap.header.stamp = ros::Time::now();
-//    mergedMap.header.frame_id = "map";
-//    mergedMap.info.width = width;
-//    mergedMap.info.height = height;
-//    mergedMap.info.resolution = resolution;
-//    mergedMap.info.origin.position.x = -height*resolution/2;
-//    mergedMap.info.origin.position.y = -width*resolution/2;
-//    mergedMap.info.origin.orientation.w = 1.0;
-//    mergedMap.info.origin.orientation.x = 0.0;
-//    mergedMap.info.origin.orientation.y = 0.0;
-//    mergedMap.info.origin.orientation.z = 0.0;
-//  	mergedMap.data.resize(ByteSize,-1);	
-
-//	  for ( int y = 0; y < msg->info.height; y++ )
-//	  {
-//		for ( int x = 0; x < msg->info.width; x++ )
-//		{
-//		  	int i = x + y * msg->info.width;
-
-//	
-//			//if cell is occupied by kinect obstacle merge cell with merged map
-//			if(msg->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED)
-//			{
-
-//				Eigen::Vector2i point(x,y);
-//				geometry_msgs::Point tmp = map_tools::fromMapCoords( point ,msg->info.origin, msg->info.resolution);
-//				point = map_tools::toMapCoords(tmp , mergedMap.info.origin, mergedMap.info.resolution);
-//				int k = point.y() * mergedMap.info.width + point.x();
-//				mergedMap.data[k] = homer_mapnav_msgs::ModifyMap::DEPTH;
-//			}
-//		}  
-//	  }
-//	
-//	
-//	m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
-} 
-
-void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr& msg) {
     MapGenerator map_saver(msg->data);
-    nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
-    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
-    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() );
+    nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+        m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+    nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+        m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+                   m_ROIManager->getList());
 }
 
-void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr& msg) {
     m_MapManager->clearMapLayers();
-    ROS_INFO_STREAM( "Loading map from file " << msg->data);
+    ROS_INFO_STREAM("Loading map from file " << msg->data);
     bool success;
     MapServer map_loader(msg->data, success);
-    if(success)
-    {
-		  ros::Rate poll_rate(10);
-		  while(m_LoadedMapPublisher.getNumSubscribers() == 0)
-		  {
-		      poll_rate.sleep();
-		  }
-		  m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
-		  m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getSLAMMap()));
-		  nav_msgs::OccupancyGrid::ConstPtr maskingMap = boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getMaskingMap());
-		  m_MaskingManager->replaceMap(map_loader.getMaskingMap());
-		  if(maskingMap->data.size() != 0)
-		  {
-		      m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
-		  }
-		  m_POIManager->replacePOIList(map_loader.getPois());
-		  m_POIManager->broadcastPoiList();
-		  m_ROIManager->replaceROIList(map_loader.getRois());
-		  m_ROIManager->broadcastRoiList();
-    }
-    else
-    {
-    	ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
+    if (success) {
+        ros::Rate poll_rate(10);
+        while (m_LoadedMapPublisher.getNumSubscribers() == 0) {
+            poll_rate.sleep();
+        }
+        m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
+        m_MapManager->updateMapLayer(
+            homer_mapnav_msgs::MapLayers::SLAM_LAYER,
+            boost::make_shared<nav_msgs::OccupancyGrid>(
+                map_loader.getSLAMMap()));
+        nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+            boost::make_shared<nav_msgs::OccupancyGrid>(
+                map_loader.getMaskingMap());
+        m_MaskingManager->replaceMap(map_loader.getMaskingMap());
+        if (maskingMap->data.size() != 0) {
+            m_MapManager->updateMapLayer(
+                homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+        }
+        m_POIManager->replacePOIList(map_loader.getPois());
+        m_POIManager->broadcastPoiList();
+        m_ROIManager->replaceROIList(map_loader.getRois());
+        m_ROIManager->broadcastRoiList();
+    } else {
+        ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
     }
 }
 
-void MapManagerNode::callbackAddPOI(const homer_mapnav_msgs::PointOfInterest::ConstPtr &msg)
-{
-  	ROS_INFO_STREAM("callbackAddPOI");
+void MapManagerNode::callbackAddPOI(
+    const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg) {
+    ROS_INFO_STREAM("callbackAddPOI");
     m_POIManager->addPointOfInterest(msg);
 }
 
-void MapManagerNode::callbackModifyPOI(const homer_mapnav_msgs::ModifyPOI::ConstPtr &msg)
-{
+void MapManagerNode::callbackModifyPOI(
+    const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg) {
     m_POIManager->modifyPointOfInterest(msg);
 }
 
-void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr& msg) {
     m_POIManager->deletePointOfInterest(msg->data);
 }
 
-void MapManagerNode::callbackAddROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
-{
+void MapManagerNode::callbackAddROI(
+    const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
     m_ROIManager->addRegionOfInterest(msg);
 }
 
-void MapManagerNode::callbackModifyROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
-{
+void MapManagerNode::callbackModifyROI(
+    const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
     m_ROIManager->modifyRegionOfInterest(msg);
 }
 
-void MapManagerNode::callbackDeleteROIbyName(const std_msgs::String::ConstPtr &msg)
-{
-  ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
-    if(m_ROIManager->deleteRegionOfInterest(msg->data))
-    {
-      ROS_INFO_STREAM("ROI deleted.");
-    }
-    else
-    {
-      ROS_WARN_STREAM("Could not delete ROI.");
+void MapManagerNode::callbackDeleteROIbyName(
+    const std_msgs::String::ConstPtr& msg) {
+    ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
+    if (m_ROIManager->deleteRegionOfInterest(msg->data)) {
+        ROS_INFO_STREAM("ROI deleted.");
+    } else {
+        ROS_WARN_STREAM("Could not delete ROI.");
     }
 }
 
-void MapManagerNode::callbackDeleteROIbyID(const std_msgs::Int32::ConstPtr &msg)
-{
+void MapManagerNode::callbackDeleteROIbyID(
+    const std_msgs::Int32::ConstPtr& msg) {
     m_ROIManager->deleteRegionOfInterest(msg->data);
 }
 
-void MapManagerNode::callbackMapVisibility(const homer_mapnav_msgs::MapLayers::ConstPtr &msg)
-{
+void MapManagerNode::callbackMapVisibility(
+    const homer_mapnav_msgs::MapLayers::ConstPtr& msg) {
     m_MapManager->toggleMapVisibility(msg->layer, msg->state);
 }
 
-void MapManagerNode::callbackModifyMap(const homer_mapnav_msgs::ModifyMap::ConstPtr &msg)
-{
-    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->modifyMap(msg);
-    if(msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER || msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV)
-    {
+void MapManagerNode::callbackModifyMap(
+    const homer_mapnav_msgs::ModifyMap::ConstPtr& msg) {
+    nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+        m_MaskingManager->modifyMap(msg);
+    if (msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER ||
+        msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV) {
         m_MaskSlamMapPublisher.publish(maskingMap);
         m_highSensitiveMap = maskingMap;
-    }
-    else
-    {
-        m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+    } else {
+        m_MapManager->updateMapLayer(
+            homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
     }
 }
 
-void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr &msg)
-{
+void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr& msg) {
     nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
-    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+                                 maskingMap);
 }
 
-void MapManagerNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
+void MapManagerNode::callbackLaserScan(
+    const sensor_msgs::LaserScan::ConstPtr& msg) {
     m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::SICK_LAYER, msg);
 }
 
-void MapManagerNode::callbackBackLaser(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
+void MapManagerNode::callbackBackLaser(
+    const sensor_msgs::LaserScan::ConstPtr& msg) {
     m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_BACK, msg);
 }
 
-void MapManagerNode::callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
-	m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
+void MapManagerNode::callbackFrontLaser(
+    const sensor_msgs::LaserScan::ConstPtr& msg) {
+    m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
 }
 
-bool MapManagerNode::getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
-                                    homer_mapnav_msgs::GetPointsOfInterest::Response& res)
-{
+bool MapManagerNode::getPOIsService(
+    homer_mapnav_msgs::GetPointsOfInterest::Request& req,
+    homer_mapnav_msgs::GetPointsOfInterest::Response& res) {
     res.poi_list.pois = m_POIManager->getList();
     return true;
 }
 
-bool MapManagerNode::getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
-                                    homer_mapnav_msgs::GetRegionsOfInterest::Response& res)
-{
+bool MapManagerNode::getROIsService(
+    homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
+    homer_mapnav_msgs::GetRegionsOfInterest::Response& res) {
     res.roi_list.rois = m_ROIManager->getList();
     return true;
 }
 
-bool MapManagerNode::pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req,
-											homer_mapnav_msgs::PointInsideRois::Response& res)
-{
-	res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
-	return true;
+bool MapManagerNode::pointInsideRoisService(
+    homer_mapnav_msgs::PointInsideRois::Request& req,
+    homer_mapnav_msgs::PointInsideRois::Response& res) {
+    res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
+    return true;
 }
 
-bool MapManagerNode::getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
-											homer_mapnav_msgs::GetRegionOfInterestName::Response& res)
-{
-	res.name = m_ROIManager->getROIName(req.roi_id);
-	return true;
+bool MapManagerNode::getROINameService(
+    homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
+    homer_mapnav_msgs::GetRegionOfInterestName::Response& res) {
+    res.name = m_ROIManager->getROIName(req.roi_id);
+    return true;
 }
 
-void MapManagerNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
-	m_MapManager->updatePose(msg);
-	if(msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) && m_roi_polling)
-	{
-		m_lastROIPoll = msg->header.stamp;
-		geometry_msgs::PointStamped posePoint; 
-		posePoint.header.frame_id="/map";
-		posePoint.header.stamp = msg->header.stamp; 
-		posePoint.point = msg->pose.position;
-		std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
-		rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
-		bool found = false;
-		for(int i = 0; i < m_ids.size(); i++)
-		{
-			found = false;
-			for(int j = 0; j < rois.size(); j++)
-			{
-				if(m_ids[i] == rois[j].id)
-				{
-			  		rois.erase (rois.begin()+j);
-			  		found = true;
-					break;
-				}
-			}
-			if(!found)
-			{
-				homer_mapnav_msgs::RoiChange change; 
-				change.id = m_ids[i];
-				change.name = m_ROIManager->getROIName(m_ids[i]);
-				change.action = false; 
-				m_RoiPollPublisher.publish(change);
-				m_ids.erase(m_ids.begin()+i);
-				i--;
-			}
-		}
-		for(int i = 0; i < rois.size(); i++)
-		{
-			homer_mapnav_msgs::RoiChange change; 
-			change.id = rois[i].id;
-			change.name = m_ROIManager->getROIName(change.id);
-			change.action = true; 
-			m_RoiPollPublisher.publish(change);
-			m_ids.push_back(rois[i].id);
-		}
-	}	
+void MapManagerNode::poseCallback(
+    const geometry_msgs::PoseStamped::ConstPtr& msg) {
+    m_MapManager->updatePose(msg);
+    if (msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) &&
+        m_roi_polling) {
+        m_lastROIPoll = msg->header.stamp;
+        geometry_msgs::PointStamped posePoint;
+        posePoint.header.frame_id = "/map";
+        posePoint.header.stamp = msg->header.stamp;
+        posePoint.point = msg->pose.position;
+        std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
+        rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
+        bool found = false;
+        for (int i = 0; i < m_ids.size(); i++) {
+            found = false;
+            for (int j = 0; j < rois.size(); j++) {
+                if (m_ids[i] == rois[j].id) {
+                    rois.erase(rois.begin() + j);
+                    found = true;
+                    break;
+                }
+            }
+            if (!found) {
+                homer_mapnav_msgs::RoiChange change;
+                change.id = m_ids[i];
+                change.name = m_ROIManager->getROIName(m_ids[i]);
+                change.action = false;
+                m_RoiPollPublisher.publish(change);
+                m_ids.erase(m_ids.begin() + i);
+                i--;
+            }
+        }
+        for (int i = 0; i < rois.size(); i++) {
+            homer_mapnav_msgs::RoiChange change;
+            change.id = rois[i].id;
+            change.name = m_ROIManager->getROIName(change.id);
+            change.action = true;
+            m_RoiPollPublisher.publish(change);
+            m_ids.push_back(rois[i].id);
+        }
+    }
 }
 
-
 bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
-					homer_mapnav_msgs::SaveMap::Response& res)
-{
-	//ROS_INFO_STREAM("Saving map "<<req->folder);
+                                    homer_mapnav_msgs::SaveMap::Response& res) {
+    // ROS_INFO_STREAM("Saving map "<<req->folder);
     MapGenerator map_saver(std::string(req.folder.data));
-    nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
-    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
-    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() );
+    nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+        m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+    nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+        m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+                   m_ROIManager->getList());
 }
 
 bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
-					homer_mapnav_msgs::LoadMap::Response& res)
-{
-
-    //load map file from config if present
+                                    homer_mapnav_msgs::LoadMap::Response& res) {
+    // load map file from config if present
     std::string mapfile = std::string(req.filename.data);
-    if(mapfile != "")
-    {
-		ROS_INFO_STREAM("Loading map with filename: "<<mapfile);
-		std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
-        mapfileMsg->data= mapfile;
-        callbackLoadMap ( mapfileMsg );
+    if (mapfile != "") {
+        ROS_INFO_STREAM("Loading map with filename: " << mapfile);
+        std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+        mapfileMsg->data = mapfile;
+        callbackLoadMap(mapfileMsg);
+    } else {
+        ROS_ERROR_STREAM("Map filename is empty. Could not load map");
     }
-	else
-	{
-		ROS_ERROR_STREAM("Map filename is empty. Could not load map");
-	}
 }
 
 bool MapManagerNode::resetMapService(std_srvs::Empty::Request& req,
-					std_srvs::Empty::Response& res)
-{
-	ROS_INFO_STREAM("Resetting current map");
+                                     std_srvs::Empty::Response& res) {
+    ROS_INFO_STREAM("Resetting current map");
     nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
-    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+                                 maskingMap);
 }
 
-int main(int argc, char** argv)
-{
+int main(int argc, char** argv) {
     ros::init(argc, argv, "map_manager");
     ros::NodeHandle nh;
 
-/*    char* pprofile    = std::getenv("MAPMANAGER_PROFILE");
-    if (pprofile)
-    {
-        ProfilerStart(pprofile);
-    }
-*/
+    /*    char* pprofile    = std::getenv("MAPMANAGER_PROFILE");
+        if (pprofile)
+        {
+            ProfilerStart(pprofile);
+        }
+    */
 
     MapManagerNode node(&nh);
 
     ros::Rate loop_rate(10);
-    while (ros::ok())
-    {
-        try
-        {
+    while (ros::ok()) {
+        try {
             ros::spinOnce();
             loop_rate.sleep();
+        } catch (exception& e) {
+            std::cout << "Exception in main loop" << e.what() << std::endl;
         }
-        catch (exception &e)
+    }
+    /*    if (pprofile)
         {
-            std::cout<<"Exception in main loop"<<e.what()<<std::endl;
+            ProfilerStop();
         }
-    }
-/*    if (pprofile)
-    {
-        ProfilerStop();
-    }
-*/
+    */
     return 0;
 }
-
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index a7e2bf69bc4aef3b76f4219b5d5069596db6195f..cf75bf3086ec610299d00aca3ac5e921ce1a23db 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -1,9 +1,8 @@
-/homer_mapping/size: 40                                          # size of one edge of the map in m. map is quadratic
-/homer_mapping/resolution: 0.05                                  # m meter per cell
-/homer_mapping/max_laser:  20.0                                   # m max range for including range into map
+/homer_mapping/size: 4                                          # size of one edge of the map in m. map is quadratic
+/homer_mapping/resolution: 0.04                               # m meter per cell
 
 #map config values
-/homer_mapping/backside_checking: false                           # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
+/homer_mapping/backside_checking: true                          # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
 /homer_mapping/obstacle_borders: true                            # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
 /homer_mapping/measure_sampling_step: 0.1                        # m Minimum distance in m between two samples for probability calculation
 
@@ -11,20 +10,20 @@
 
 /particlefilter/error_values/rotation_error_rotating: 10.0       # percent
 /particlefilter/error_values/rotation_error_translating: 2.0     # degrees per meter
-/particlefilter/error_values/translation_error_translating: 10.0 # percent
-/particlefilter/error_values/translation_error_rotating: 0.05    # m per degree
-/particlefilter/error_values/move_jitter_while_turning: 0.1      # m per degree
+/particlefilter/error_values/translation_error_translating: 1.0 # percent
+/particlefilter/error_values/translation_error_rotating: 0.09    # m per degree
+/particlefilter/error_values/move_jitter_while_turning: 0.05      # m per degree
 
 /particlefilter/hyper_slamfilter/particlefilter_num: 1
 
 /particlefilter/particle_num: 1000
-/particlefilter/max_rotation_per_second: 0.4                     # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
+/particlefilter/max_rotation_per_second: 0.1                     # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
 /particlefilter/wait_time: 0.05                                  # minimum time to wait between two slam steps in seconds
 
 #the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed
 /particlefilter/update_min_move_angle: 2                         # degrees
 /particlefilter/update_min_move_dist: 0.05                       # m
-/particlefilter/max_update_interval: 0.5                         # sec
+/particlefilter/max_update_interval: 0.2                         # sec
 
 /selflocalization/scatter_var_xy: 0.1                            # m
 /selflocalization/scatter_var_theta: 0.2                         # radiants
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 0235e8604a89f3ea651bd3a1815eb393e0213cc3..63f51b978d6c4d74dab3ad59095d7aeb6b20acd6 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -13,6 +13,7 @@
 #include <homer_nav_libs/Math/Box2D.h>
 
 #include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
 #include <tf/transform_listener.h>
 
 #include <sensor_msgs/LaserScan.h>
@@ -91,7 +92,7 @@ class OccupancyMap {
     /**
      * Constructor for a loaded map.
      */
-    OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion);
+    OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion);
 
     /**
      * Copy constructor, copies all members inclusive the arrays that lie behind the pointers.
@@ -170,7 +171,7 @@ class OccupancyMap {
     float computeScore( Pose robotPose, std::vector<MeasurePoint> measurePoints );
 
     /**
-     * @return QImage of size m_PixelSize, m_PixelSize with values of m_OccupancyProbability scaled to 0-254
+     * @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254
      */
     QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
 
@@ -187,9 +188,9 @@ class OccupancyMap {
      * @param[out] data vector containing occupancy probabilities. 0 = free, 100 = occupied, -1 = NOT_KNOWN
      * @param[out] width Width of data array
      * @param[out] height Height of data array
-     * @param[out] resolution Resolution of the map (m_Resolution)
+     * @param[out] resolution Resolution of the map (m_metaData.resolution)
      */
-    void getOccupancyProbabilityImage(vector<int8_t> &data, int& width, int& height, float &resolution);
+    void getOccupancyProbabilityImage(vector<int8_t> &data, nav_msgs::MapMetaData& metaData);
 
     /**
      * This method marks free the position of the robot (according to its dimensions).
@@ -219,6 +220,8 @@ class OccupancyMap {
      */
      void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
 
+	 void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down );
+
 
   protected:
 
@@ -297,22 +300,14 @@ class OccupancyMap {
      */
      void cleanUp();
 
-    /**
-     * Stores the size of one map pixel in m.
-     */
-    float m_Resolution;
+	/**
+	 * Stores the metadata of the map
+	 */
+	nav_msgs::MapMetaData m_metaData;
 
-    /**
-     * Stores the origin of the map
-     */
-    geometry_msgs::Pose m_Origin;
-    /**
-     * Stores the width of the map in cell numbers.
-     */
-    int m_PixelSize;
 
     /**
-     * Stores the size of the map arrays, i.e. m_PixelSize * m_PixelSize
+     * Stores the size of the map arrays, i.e. m_metaData.width * m_metaData.height
      * for faster computation.
      */
     unsigned m_ByteSize;
@@ -329,9 +324,6 @@ class OccupancyMap {
     // Counts how often a pixel is hit by a measurement that says the pixel is occupied.
     unsigned short* m_OccupancyCount;
 
-    // Counts how often a cell is marked as inaccessible via markInaccessible()
-    unsigned char* m_InaccessibleCount;
-
     // Used for setting flags for cells, that have been modified during the current update.
     unsigned char* m_CurrentChanges;
     
@@ -341,10 +333,6 @@ class OccupancyMap {
     /**
      * Store values from config files.
      */
-    // maximum valid range of one laser measurement
-    float m_LaserMaxRange;
-    //minimum valid range of one laser measurement
-    float m_LaserMinRange;
     //minimum range classified as free in case of errorneous laser measurement
     float m_FreeReadingDistance;
     //enables checking to avoid matching front- and backside of an obstacle, e.g. wall
@@ -357,9 +345,6 @@ class OccupancyMap {
     //bool to reset high_sensitive Areas on the next iteration
     bool m_reset_high;
 
-    //position of the laser scaner in base_link frame
-    geometry_msgs::Point m_LaserPos;
-
     /**
      * Defines a bounding box around the changes in the current map.
      */
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index a712fb390b06c70a7a311674f4ca6492fdb32a6e..89026615dcd140e77e3fb11159c6ebce03f500d0 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -312,6 +312,8 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
      */
     ros::Time m_LastUpdateTime;
 
+    ros::Time m_LastMoveTime;
+
     /**
      * Calculates the square of given input f
      * @param f input
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 7318e38aaf7e129b1acb2d75445a503f81e4d994..515128a22715094ec3338dcd8a4bbde13989cdfb 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -37,18 +37,35 @@ const int LOADED_MEASURECOUNT = 10;
 
 OccupancyMap::OccupancyMap()
 {
+  float mapSize, resolution;
+  ros::param::get("/homer_mapping/size", mapSize);
+  ros::param::get("/homer_mapping/resolution", resolution);
+
+  //add one safety pixel
+  m_metaData.resolution = resolution;
+  m_metaData.width = mapSize / m_metaData.resolution + 1;
+  m_metaData.height = mapSize / m_metaData.resolution + 1;
+  m_ByteSize = m_metaData.width * m_metaData.height;
+
+  m_metaData.origin.position.x = - (m_metaData.width * m_metaData.resolution / 2.0);
+  m_metaData.origin.position.y = - (m_metaData.height * m_metaData.resolution / 2.0);
+  m_metaData.origin.orientation.w = 1.0;
+  m_metaData.origin.orientation.x = 0.0;
+  m_metaData.origin.orientation.y = 0.0;
+  m_metaData.origin.orientation.z = 0.0;
   initMembers();
 }
 
-OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion)
+OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion)
 {
+    m_metaData.origin = origin;
+    m_metaData.resolution = resolution;
+	m_metaData.width = width;
+	m_metaData.height = height;
+    m_ByteSize = m_metaData.width * m_metaData.height;
     initMembers();
 
 
-    m_Origin = origin;
-    m_Resolution = resolution;
-    m_PixelSize = pixelSize;
-    m_ByteSize = pixelSize * pixelSize;
     m_ExploredRegion = exploredRegion;
     m_ChangedRegion = exploredRegion;
 
@@ -74,10 +91,7 @@ OccupancyMap::OccupancyMap ( const OccupancyMap& occupancyMap )
   m_MeasurementCount = 0;
   m_OccupancyCount = 0;
   m_CurrentChanges = 0;
-  m_InaccessibleCount = 0;
   m_HighSensitive = 0;
-  m_LaserMaxRange = 0;
-  m_LaserMinRange = 0;
 
   *this = occupancyMap;
 }
@@ -89,21 +103,6 @@ OccupancyMap::~OccupancyMap()
 
 void OccupancyMap::initMembers()
 {
-  float mapSize;
-  ros::param::get("/homer_mapping/size", mapSize);
-  ros::param::get("/homer_mapping/resolution", m_Resolution);
-  ros::param::param("/homer_mapping/max_laser", m_LaserMaxRange, (float) 8.0);
-
-  //add one safety pixel
-  m_PixelSize = mapSize / m_Resolution + 1;
-  m_ByteSize = m_PixelSize * m_PixelSize;
-
-  m_Origin.position.x = -m_PixelSize*m_Resolution/2;
-  m_Origin.position.y = -m_PixelSize*m_Resolution/2;
-  m_Origin.orientation.w = 1.0;
-  m_Origin.orientation.x = 0.0;
-  m_Origin.orientation.y = 0.0;
-  m_Origin.orientation.z = 0.0;
 
   ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
   ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
@@ -114,7 +113,6 @@ void OccupancyMap::initMembers()
   m_MeasurementCount = new unsigned short[m_ByteSize];
   m_OccupancyCount = new unsigned short[m_ByteSize];
   m_CurrentChanges = new unsigned char[m_ByteSize];
-  m_InaccessibleCount = new unsigned char[m_ByteSize];
   m_HighSensitive = new unsigned short[m_ByteSize];
   for ( unsigned i=0; i<m_ByteSize; i++ )
   {
@@ -122,11 +120,10 @@ void OccupancyMap::initMembers()
     m_OccupancyCount[i]=0;
     m_MeasurementCount[i]=0;
     m_CurrentChanges[i]=NO_CHANGE;
-    m_InaccessibleCount[i]=0;
     m_HighSensitive[i] = 0;
   }
 
-  m_ExploredRegion=Box2D<int> ( m_PixelSize/2.1, m_PixelSize/2.1, m_PixelSize/1.9, m_PixelSize/1.9 );
+  m_ExploredRegion=Box2D<int> ( m_metaData.width/2.1, m_metaData.height/2.1, m_metaData.width/1.9, m_metaData.height/1.9 );
   maximizeChangedRegion();
   
   try
@@ -134,8 +131,11 @@ void OccupancyMap::initMembers()
   	bool got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
   	while(!got_transform); 	
   	{
-  		ROS_ERROR_STREAM("need transformation from base_link to laser!");
   		got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
+  		if(!got_transform)
+		{
+			ROS_ERROR_STREAM("need transformation from base_link to laser!");
+		}
   	}
   	
     m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0), m_laserTransform);
@@ -152,9 +152,9 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
   // free allocated memory
   cleanUp();
 
-  m_Resolution = occupancyMap.m_Resolution;
+  m_metaData = occupancyMap.m_metaData;
+
   m_ExploredRegion =  occupancyMap.m_ExploredRegion;
-  m_PixelSize = occupancyMap.m_PixelSize;
   m_ByteSize = occupancyMap.m_ByteSize;
 
   ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
@@ -164,7 +164,6 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
   m_MeasurementCount = new unsigned short[m_ByteSize];
   m_OccupancyCount = new unsigned short[m_ByteSize];
   m_CurrentChanges = new unsigned char[m_ByteSize];
-  m_InaccessibleCount = new unsigned char[m_ByteSize];
   m_HighSensitive = new unsigned short[m_ByteSize];
 
   // copy array data
@@ -172,30 +171,93 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
   memcpy ( m_MeasurementCount, occupancyMap.m_MeasurementCount, m_ByteSize * sizeof ( *m_MeasurementCount ) );
   memcpy ( m_OccupancyCount, occupancyMap.m_OccupancyCount, m_ByteSize * sizeof ( *m_OccupancyCount ) );
   memcpy ( m_CurrentChanges, occupancyMap.m_CurrentChanges, m_ByteSize * sizeof ( *m_CurrentChanges ) );
-  memcpy ( m_InaccessibleCount, occupancyMap.m_InaccessibleCount, m_ByteSize * sizeof ( *m_InaccessibleCount ) );
   memcpy ( m_HighSensitive, occupancyMap.m_HighSensitive, m_ByteSize * sizeof ( *m_HighSensitive) );
 
 
   return *this;
 }
 
+void OccupancyMap::changeMapSize( int x_add_left, int y_add_up, int x_add_right, int y_add_down)
+{
+  int new_width  = m_metaData.width + x_add_left + x_add_right;
+  int new_height = m_metaData.height + y_add_up + y_add_down;
+
+  m_ByteSize = new_width * new_height;
+  // allocate all arrays
+  float* OccupancyProbability = new float[m_ByteSize];
+  unsigned short* MeasurementCount = new unsigned short[m_ByteSize];
+  unsigned short* OccupancyCount = new unsigned short[m_ByteSize];
+  unsigned char* CurrentChanges = new unsigned char[m_ByteSize];
+  unsigned short* HighSensitive = new unsigned short[m_ByteSize];
+
+  for ( unsigned i=0; i<m_ByteSize; i++ )
+  {
+    OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
+    OccupancyCount[i]=0;
+    MeasurementCount[i]=0;
+    CurrentChanges[i]=NO_CHANGE;
+    HighSensitive[i] = 0;
+  }
+
+  for(int y = 0; y < m_metaData.height; y++)
+  {
+	  for(int x = 0; x < m_metaData.width; x++)
+	  {
+		  int i = y * m_metaData.width + x;
+		  int in = (y + y_add_up) * new_width + (x + x_add_left);  
+		  OccupancyProbability[in] = m_OccupancyProbability[i];
+		  MeasurementCount[in] = m_MeasurementCount[i]; 
+		  OccupancyCount[in] =  m_OccupancyCount[i];  
+		  CurrentChanges[in] =  m_CurrentChanges[i];  
+		  HighSensitive[in] = m_HighSensitive[i]; 
+	  }
+  }
+
+  m_ExploredRegion.setMinX( m_ExploredRegion.minX() + x_add_left);
+  m_ExploredRegion.setMaxX( m_ExploredRegion.maxX() + x_add_left);
+  m_ExploredRegion.setMinY( m_ExploredRegion.minY() + y_add_up);
+  m_ExploredRegion.setMaxY( m_ExploredRegion.maxY() + y_add_up);
+
+  m_ChangedRegion.setMinX( m_ChangedRegion.minX() + x_add_left);
+  m_ChangedRegion.setMaxX( m_ChangedRegion.maxX() + x_add_left);
+  m_ChangedRegion.setMinY( m_ChangedRegion.minY() + y_add_up);
+  m_ChangedRegion.setMaxY( m_ChangedRegion.maxY() + y_add_up);
+
+
+  m_metaData.width = new_width; 
+  m_metaData.height = new_height;
+  m_metaData.origin.position.x -= (x_add_left ) * m_metaData.resolution;
+  m_metaData.origin.position.y -= (y_add_up ) * m_metaData.resolution;
+
+  cleanUp();
+
+  m_OccupancyProbability = OccupancyProbability;
+  m_MeasurementCount = MeasurementCount; 
+  m_OccupancyCount =  OccupancyCount;  
+  m_CurrentChanges =  CurrentChanges;  
+  m_HighSensitive = HighSensitive; 
+
+ 
+
+}
+
 int OccupancyMap::width() const
 {
-  return m_PixelSize;
+  return m_metaData.width;
 }
 
 int OccupancyMap::height() const
 {
-  return m_PixelSize;
+  return m_metaData.height;
 }
 
 float OccupancyMap::getOccupancyProbability ( Eigen::Vector2i p )
 {
-  unsigned offset = m_PixelSize * p.y() + p.x();
-  if ( offset > unsigned ( m_ByteSize ) )
+  if(p.y() >= m_metaData.height || p.x() >= m_metaData.width)
   {
-    return UNKNOWN_LIKELIHOOD;
+  	return UNKNOWN_LIKELIHOOD;
   }
+  unsigned offset = m_metaData.width * p.y() + p.x();
   return m_OccupancyProbability[ offset ];
 }
 
@@ -209,12 +271,22 @@ void OccupancyMap::computeOccupancyProbabilities()
 {
   for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
   {
-    int yOffset = m_PixelSize * y;
+    int yOffset = m_metaData.width * y;
     for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
     {
       int i = x + yOffset;
       if ( m_MeasurementCount[i] > 0 )
       {
+		//int maxCount = 100; //TODO param
+		//if(m_MeasurementCount[i] > maxCount * 2 -1)
+		//{
+			//int scalingFactor = m_MeasurementCount[i] / maxCount;
+			//if ( scalingFactor != 0 )
+			//{
+				//m_MeasurementCount[i] /= scalingFactor;
+				//m_OccupancyCount[i] /= scalingFactor;
+			//}
+		//}
         m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float>  	( m_MeasurementCount[i] );
 		if (m_HighSensitive[i] == 1)
 		{
@@ -250,14 +322,6 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData,
 {
   m_latestMapTransform = transform;
   markRobotPositionFree();
-  ros::Time stamp = laserData->header.stamp;
-
-  //m_LaserMaxRange = laserData->range_max;
-  m_LaserMinRange = laserData->range_min;
-
-
-  m_LaserPos.x = m_laserTransform.getOrigin().getX();
-  m_LaserPos.y = m_laserTransform.getOrigin().getY();
 
   std::vector<RangeMeasurement> ranges;
   ranges.reserve ( laserData->ranges.size() );
@@ -267,11 +331,12 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData,
   float lastValidRange=m_FreeReadingDistance;
 
   RangeMeasurement rangeMeasurement;
-  rangeMeasurement.sensorPos = m_LaserPos;
+  rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
+  rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
   
   for ( unsigned int i = 0; i < laserData->ranges.size(); i++ )
   {
-    if ( ( laserData->ranges[i] >= m_LaserMinRange ) && ( laserData->ranges[i] <= m_LaserMaxRange ) )
+    if ( ( laserData->ranges[i] >= laserData->range_min ) && ( laserData->ranges[i] <= laserData->range_max ) )
     {
       //if we're at the end of an errorneous segment, interpolate
       //between last valid point and current point
@@ -279,56 +344,43 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData,
       {
         //smaller of the two ranges belonging to end points
         float range = Math::min ( lastValidRange, laserData->ranges[i] );
-        range -= m_Resolution * 2;
-
+        range -= m_metaData.resolution * 2;
         if ( range < m_FreeReadingDistance )
         {
           range = m_FreeReadingDistance;
         }
         else
-          if ( range > m_LaserMaxRange )
+          if ( range > laserData->range_max )
           {
-            range = m_LaserMaxRange;
+            range = laserData->range_max;
           }
-
         //choose smaller range
         for ( unsigned j=lastValidIndex+1; j<i; j++ )
         {  
 			float alpha = laserData->angle_min + j * laserData->angle_increment;
-			geometry_msgs::Point point;
 			tf::Vector3 pin;
 			tf::Vector3 pout;
 			pin.setX( cos(alpha) * range);
 			pin.setY( sin(alpha) * range);
 			pin.setZ(0);
-
 			pout = m_laserTransform * pin;
-
-			point.x = pout.x();
-			point.y = pout.y();
-
-			rangeMeasurement.endPos = point;
+			rangeMeasurement.endPos.x = pout.x();
+			rangeMeasurement.endPos.y = pout.y();
 			rangeMeasurement.free = true;
 			ranges.push_back ( rangeMeasurement );
         }                             
       }
 	  float alpha = laserData->angle_min + i * laserData->angle_increment;
-	  geometry_msgs::Point point;
 	  tf::Vector3 pin;
 	  tf::Vector3 pout;
 	  pin.setX( cos(alpha) * laserData->ranges[i]);
 	  pin.setY( sin(alpha) * laserData->ranges[i]);
 	  pin.setZ(0);
-
 	  pout = m_laserTransform * pin;
-
-	  point.x = pout.x();
-	  point.y = pout.y();
-
-	  rangeMeasurement.endPos = point;
+	  rangeMeasurement.endPos.x = pout.x();
+	  rangeMeasurement.endPos.y = pout.y();
       rangeMeasurement.free = false;
       ranges.push_back ( rangeMeasurement );
-
       errorFound=false;
       lastValidIndex=i;
       lastValidRange=laserData->ranges[i];
@@ -339,113 +391,145 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData,
     }
   }
 
-//  if ( errorFound )
-//  {
-//    for ( unsigned j=lastValidIndex+1; j<laserData->ranges.size(); j++ )
-//    {
-//      rangeMeasurement.endPos = map_tools::laser_range_to_point(m_FreeReadingDistance, j, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link"); //
-
-//      rangeMeasurement.free = true;
-//      ranges.push_back ( rangeMeasurement );
-//    }
-//  }
-
   insertRanges ( ranges , laserData->header.stamp);
+
 }
 
 
 void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time stamp )
 {
+	if(ranges.size() < 1)
+	{
+		return;
+	}
   clearChanges();
 
   Eigen::Vector2i lastEndPixel;
+  int need_x_left = 0;
+  int need_x_right = 0; 
+  int need_y_up = 0;
+  int need_y_down = 0;
 
-  //paint safety borders
-  if ( m_ObstacleBorders )
-  {
-    for ( unsigned i=0; i<ranges.size(); i++ )
-    {
-      geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
-      Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
+  std::vector<Eigen::Vector2i> map_pixel;
 
-      for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
-      {
-        for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
-        {
-          unsigned offset=x+m_PixelSize*y;
-          if ( offset < unsigned ( m_ByteSize ) )
-          {
-            m_CurrentChanges[ offset ] = SAFETY_BORDER;
-          }
-        }
-      }
-    }
-  }
-  //paint safety ranges
-  for ( unsigned i=0; i<ranges.size(); i++ )
+  for(int i = 0; i < ranges.size(); i++)
   {
-      geometry_msgs::Point startPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
-      Eigen::Vector2i startPixel = map_tools::toMapCoords(startPosWorld, m_Origin, m_Resolution);
-    geometry_msgs::Point endPos;
-    endPos.x = ranges[i].endPos.x * 4;
-    endPos.y = ranges[i].endPos.y * 4;
-
-    geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform);
-    Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
-
-    if(endPixel.x() < 0) endPixel.x() = 0;
-    if(endPixel.y() < 0) endPixel.y() = 0;
-    if(endPixel.x() >= m_PixelSize) endPixel.x() = m_PixelSize - 1;
-    if(endPixel.y() >= m_PixelSize) endPixel.y() = m_PixelSize - 1;
-
-    drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
+	  geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+	  map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution));
   }
+	  geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
+	  Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_metaData.origin, m_metaData.resolution);
+	  m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
+
+  ////paint safety borders
+  //if ( m_ObstacleBorders )
+  //{
+	  //for ( unsigned i=0; i<ranges.size(); i++ )
+	  //{
+		  //Eigen::Vector2i endPixel = map_pixel[i];
+
+		  //for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
+		  //{
+			  //if(y > m_metaData.height) continue;
+			  //for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
+			  //{
+				  //if(x > m_metaData.width) continue;
+				  //unsigned offset=x+m_metaData.width*y;
+				  //if ( offset < unsigned ( m_ByteSize ) )
+				  //{
+					  //m_CurrentChanges[ offset ] = SAFETY_BORDER;
+				  //}
+			  //}
+		  //}
+	  //}
+  //}
+  ////paint safety ranges
+  //for ( unsigned i=0; i<ranges.size(); i++ )
+  //{
+	  //Eigen::Vector2i startPixel = map_pixel[i];
+	  //geometry_msgs::Point endPos;
+	  //endPos.x = ranges[i].endPos.x * 4;
+	  //endPos.y = ranges[i].endPos.y * 4;
+
+	  //geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform);
+	  //Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
+
+
+	  //if(endPixel.x() < 0) endPixel.x() = 0;
+	  //if(endPixel.y() < 0) endPixel.y() = 0;
+	  //if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
+	  //if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height - 1;
+
+	  //drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
+  //}
 
   //paint end pixels
   for ( unsigned i=0; i<ranges.size(); i++ )
   {
-    if ( !ranges[i].free )
-    {
-      geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
-      Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
-      if ( endPixel != lastEndPixel )
-      {
-        unsigned offset = endPixel.x() + m_PixelSize * endPixel.y();
-        if ( offset < m_ByteSize )
-        {
-          m_CurrentChanges[ offset ] = ::OCCUPIED;
-        }
-      }
-      lastEndPixel=endPixel;
-      }
-  }
-
-  //paint free ranges
-  geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
-  Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_Origin, m_Resolution);
-  
-  for ( unsigned i=0; i<ranges.size(); i++ )
-  {
-    geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
-    Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
-
-    m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
-    m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
-
-    if ( endPixel != lastEndPixel )
-    {
-        drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
-    }
+	  Eigen::Vector2i endPixel = map_pixel[i];
+
+	  if ( endPixel != lastEndPixel )
+	  {
+		  bool outside = false;
+		  if(endPixel.x() >= m_metaData.width)
+		  {	
+			  need_x_right = std::max((int)( endPixel.x() - m_metaData.width + 10), need_x_right);
+			  outside = true;
+		  }
+		  if(endPixel.x() < 0)
+		  {
+			  need_x_left =  std::max((int)(-endPixel.x()) + 10, need_x_left);
+			  outside = true;
+		  }
+		  if(endPixel.y() >= m_metaData.height)
+		  {
+			  need_y_down = std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
+			  outside = true;
+		  }
+		  if(endPixel.y() < 0)
+		  {
+			  need_y_up = std::max((int)(- endPixel.y()) + 10, need_y_up);
+			  outside = true;
+		  }
+		  if(outside)
+		  {
+			  continue;
+		  }
+		  m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
+		  //paint free ranges
+		  drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
 
-    lastEndPixel=endPixel;
+		  if ( !ranges[i].free )
+		  {
+			  unsigned offset = endPixel.x() + m_metaData.width * endPixel.y();
+			  m_CurrentChanges[ offset ] = ::OCCUPIED;
+		  }
+	  }
+	  lastEndPixel=endPixel;
   }
 
-  m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) );
+  m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
   m_ExploredRegion.enclose ( m_ChangedRegion );
   applyChanges();
   computeOccupancyProbabilities();
+  if(need_x_left + need_x_right + need_y_down + need_y_up > 0)
+  {
+      //keep square aspect ration till homer_gui can handle other maps
+      //int need_x = need_x_left + need_x_right; 
+      //int need_y = need_y_up + need_y_down;
+      //if(need_x > need_y)
+      //{
+        //need_y_down += need_x - need_y;
+      //}
+      //else if (need_y > need_x)
+      //{
+        //need_x_right += need_y - need_x;
+      //}
+
+	  ROS_INFO_STREAM("new map size!");
+	  ROS_INFO_STREAM(" "<<need_x_left<<" "<<need_y_up<<" "<<need_x_right<<" "<<need_y_down);
+	  changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
+  }
 }
 
 double OccupancyMap::contrastFromProbability ( int8_t prob )
@@ -473,7 +557,7 @@ double OccupancyMap::evaluateByContrast()
   {
     for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
     {
-      int i = x + y * m_PixelSize;
+      int i = x + y * m_metaData.width;
       if ( m_MeasurementCount [i] > 1 )
       {
         int prob = m_OccupancyProbability[i] * 100;
@@ -501,16 +585,13 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst
 
   double minDist = m_MeasureSamplingStep;
 
-  //m_LaserMaxRange = laserData->range_max;
-  m_LaserMinRange = laserData->range_min;
-
   Point2D lastHitPos;
   Point2D lastUsedHitPos;
 
   //extract points for measuring
   for ( unsigned int i=0; i < laserData->ranges.size(); i++ )
   {
-    if ( laserData->ranges[i] <= m_LaserMaxRange && laserData->ranges[i] >= m_LaserMinRange )
+    if ( laserData->ranges[i] <= laserData->range_max && laserData->ranges[i] >= laserData->range_min )
     {
 		float alpha = laserData->angle_min + i * laserData->angle_increment;
 		tf::Vector3 pin;
@@ -528,7 +609,7 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst
         MeasurePoint p;
         //preserve borders of segments
         if ( ( i!=0 ) &&
-                ( lastUsedHitPos.distance ( lastHitPos ) > m_Resolution*0.5 ) &&
+                ( lastUsedHitPos.distance ( lastHitPos ) > m_metaData.resolution*0.5 ) &&
                 ( hitPos.distance ( lastHitPos ) >= minDist*1.5 ) )
         {
           p.hitPos = lastHitPos;
@@ -579,7 +660,7 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst
 
     CVec2 normal = diff.rotate ( Math::Pi * 0.5 );
     normal.normalize();
-    normal *= m_Resolution * sqrt ( 2.0 ) * 10.0;
+    normal *= m_metaData.resolution * sqrt ( 2.0 ) * 10.0;
 
     result[i].frontPos = result[i].hitPos + normal;
   }
@@ -609,8 +690,8 @@ float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> mea
     hitPos.x = x;
     hitPos.y = y;
 
-    Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_Origin, m_Resolution);
-    hitOffset = hitPixel.x() + m_PixelSize*hitPixel.y();
+    Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin, m_metaData.resolution);
+    hitOffset = hitPixel.x() + m_metaData.width*hitPixel.y();
 
     //avoid multiple measuring of same pixel or unknown pixel
     if ( ( hitOffset == lastOffset ) || ( hitOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[hitOffset] == 0 ) )
@@ -627,8 +708,8 @@ float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> mea
       frontPos.x = x;
       frontPos.y = y;
 
-      Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_Origin, m_Resolution);
-      frontOffset = frontPixel.x() + m_PixelSize*frontPixel.y();
+      Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_metaData.origin, m_metaData.resolution);
+      frontOffset = frontPixel.x() + m_metaData.width*frontPixel.y();
 
       if ( ( frontOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[frontOffset] == 0 ) )
       {
@@ -691,10 +772,13 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
   // compute cells
   for ( t = 0; t < dist; t++ )
   {
-    int index = x + m_PixelSize * y;
+    int index = x + m_metaData.width * y;
     // set flag to free if no flag is set
     // (do not overwrite occupied cells)
-    if(index < 0) continue;
+    if(index < 0 || index > m_ByteSize)
+	{
+		continue;
+	}
     if ( data[index] == NO_CHANGE )
     {
       data[index] = value;
@@ -721,10 +805,10 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
 
 void OccupancyMap::applyChanges()
 {
-  for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
+  for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
   {
-    int yOffset = m_PixelSize * y;
-    for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
+    int yOffset = m_metaData.width * y;
+    for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
     {
       int i = x + yOffset;
       if ( ( m_CurrentChanges[i] == ::FREE || m_CurrentChanges[i] == ::OCCUPIED ) && m_MeasurementCount[i] < SHRT_MAX )
@@ -733,53 +817,72 @@ void OccupancyMap::applyChanges()
       }
       if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
       {
-        m_OccupancyCount[i]++;
+		
+        //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]++ ;
       }
     }
   }
+  for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
+  {
+    int yOffset = m_metaData.width * y;
+    for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
+    {
+      int i = x + yOffset;
+		if(m_OccupancyCount[i]> m_MeasurementCount[i])
+	m_OccupancyCount[i]=m_MeasurementCount[i];
+}}
 }
 
 void OccupancyMap::clearChanges()
 {
   m_ChangedRegion.expand ( 2 );
-  m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) );
+  m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
   for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
   {
-    int yOffset = m_PixelSize * y;
+    int yOffset = m_metaData.width * y;
     for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
     {
       int i = x + yOffset;
       m_CurrentChanges[i] = NO_CHANGE;
     }
   }
-  m_ChangedRegion=Box2D<int> ( m_PixelSize - 1, m_PixelSize - 1, 0, 0 );
+  m_ChangedRegion=Box2D<int> ( m_metaData.width - 1, m_metaData.height - 1, 0, 0 );
 }
 
 void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p )
 {
-  unsigned index = p.x() + m_PixelSize * p.y();
-  if ( index < m_ByteSize )
-  {
-    if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
-    {
-      m_CurrentChanges[index] = ::FREE;
-      m_MeasurementCount[index]++;
-    }
-  }
-  else
-  {
-    ROS_ERROR( "Index out of bounds: x = %d, y = %d", p.x(), p.y() );
-  }
+	if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+	{
+		return;
+	}
+	unsigned index = p.x() + m_metaData.width * p.y();
+	if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
+	{
+		m_CurrentChanges[index] = ::FREE;
+		m_MeasurementCount[index]++;
+	}
 }
 
 void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p )
 {
-  int index = p.x() + m_PixelSize * p.y();
-  if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX )
-  {
-    m_CurrentChanges[index] = ::OCCUPIED;
-    m_OccupancyCount[index]++;
-  }
+	if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+	{
+		return;
+	}
+	unsigned index = p.x() + m_metaData.width * p.y();
+	if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX )
+	{
+		m_CurrentChanges[index] = ::OCCUPIED;
+		m_OccupancyCount[index]++;
+	}
 }
 
 void OccupancyMap::scaleDownCounts ( int maxCount )
@@ -790,7 +893,6 @@ void OccupancyMap::scaleDownCounts ( int maxCount )
     ROS_WARN("WARNING: argument maxCount is choosen to small, resetting map.");
     memset ( m_MeasurementCount, 0, m_ByteSize );
     memset ( m_OccupancyCount, 0, m_ByteSize );
-    memset ( m_InaccessibleCount, 0, m_ByteSize );
   }
   else
   {
@@ -801,11 +903,6 @@ void OccupancyMap::scaleDownCounts ( int maxCount )
       {
         m_MeasurementCount[i] /= scalingFactor;
         m_OccupancyCount[i] /= scalingFactor;
-        m_InaccessibleCount[i] /= scalingFactor;
-      }
-      if ( m_InaccessibleCount[i] > maxCount )
-      {
-        m_InaccessibleCount[i] = maxCount;
       }
     }
   }
@@ -822,14 +919,14 @@ void OccupancyMap::markRobotPositionFree()
   point.y = 0;
   point.z = 0;
   geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform);
-  Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution);
+  Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
 
-  int width = 0.4 / m_Resolution;
+  int width = 0.35 / m_metaData.resolution;
   for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ )
   {
     for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ )
     {
-      incrementMeasurementCount ( Eigen::Vector2i ( i, j ) );
+      incrementMeasurementCount ( Eigen::Vector2i ( j, i ) );
     }
   }
   Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width );
@@ -840,12 +937,12 @@ void OccupancyMap::markRobotPositionFree()
 
 QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool showInaccessible ) const
 {
-  QImage retImage ( m_PixelSize, m_PixelSize, QImage::Format_RGB32 );
-  for ( int y = 0; y < m_PixelSize; y++ )
+  QImage retImage ( m_metaData.width, m_metaData.height, QImage::Format_RGB32 );
+  for ( int y = 0; y < m_metaData.height; y++ )
   {
-    for ( int x = 0; x < m_PixelSize; x++ )
+    for ( int x = 0; x < m_metaData.width; x++ )
     {
-      int index = x + y * m_PixelSize;
+      int index = x + y * m_metaData.width;
       int value = UNKNOWN;
       if ( m_MeasurementCount[index] > 0 )
       {
@@ -855,26 +952,20 @@ QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool show
           value = static_cast<int> ( ( 0.75 + 0.025 * m_MeasurementCount[index] ) * ( 1.0 - m_OccupancyProbability[index] ) * 255 );
         }
       }
-      if ( showInaccessible && m_InaccessibleCount[index] >= 2 )
-      {
-        value = 0;
-      }
       retImage.setPixel ( x, y, qRgb ( value, value, value ) );
     }
   }
   return retImage;
 }
 
-void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, int& width, int& height, float& resolution )
+void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, nav_msgs::MapMetaData& metaData)
 {
-  width = m_PixelSize;
-  height = m_PixelSize;
-  resolution = m_Resolution;
-  data.resize(m_PixelSize * m_PixelSize);
+  metaData = m_metaData;
+  data.resize(m_metaData.width * m_metaData.height);
   std::fill(data.begin(), data.end(), (int8_t)NOT_SEEN_YET); //note: linker error without strange cast from int8_t to int8_t
   for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
   {
-    int yOffset = m_PixelSize * y;
+    int yOffset = m_metaData.width * y;
     for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
     {
       int i = x + yOffset;
@@ -882,12 +973,6 @@ void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, int& wid
       {
         continue;
       }
-      // set inaccessible points to black
-      if ( m_InaccessibleCount[i] >= 2 )
-      {
-        data[i] = 99;
-        continue;
-      }
       if(m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD)
       {
           data[i] = NOT_SEEN_YET;
@@ -966,10 +1051,6 @@ void OccupancyMap::cleanUp()
   {
     delete[] m_CurrentChanges;
   }
-  if ( m_InaccessibleCount )
-  {
-    delete[] m_InaccessibleCount;
-  }
   if ( m_HighSensitive ) 
   {
     delete[] m_HighSensitive;
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 628ae187df2b2ea8d1bb131a3ac8c12083de87b6..1e58a7278480b6d6b9054463e2d50bdcb08f905a 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -4,7 +4,7 @@
 // minimum move for translation in m
 const float MIN_MOVE_DISTANCE2 = 0.001 * 0.001;
 // minimum turn in radiants
-const float MIN_TURN_DISTANCE2 = 0.001 * 0.001;
+const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
 
 const float M_2PI = 2 * M_PI;
 
@@ -50,6 +50,7 @@ SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( part
 
   m_EffectiveParticleNum = m_ParticleNum;
   m_LastUpdateTime = ros::Time(0);
+  m_LastMoveTime = ros::Time::now();
 }
 
 SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle> ( slamFilter.m_ParticleNum )
@@ -315,24 +316,29 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD
 
   Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
 
-  // do not resample if move to small
-  if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 )
+  // do not resample if move to small and last move is min 0.5 sec away
+  if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0 )
+  //if(false)
   {
-    ROS_DEBUG_STREAM( "Move too small, will not resample." );
-    if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
-    {
-      resample();
-      ROS_INFO_STREAM( "Particles too scattered, resampling." );
-      // filter steps
+	ROS_DEBUG_STREAM( "Move too small, will not resample." );
+	if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
+	{
+	  resample();
+	  ROS_INFO_STREAM( "Particles too scattered, resampling." );
+	  // filter steps
 	  drift();
 	  measure();
 	  normalize();
 
 	  sort ( 0, m_ParticleNum - 1 );
-    }
+	}
   }
   else
   {
+      if(!( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 ))
+      {
+        m_LastMoveTime = ros::Time::now();
+      }
     resample();
     // filter steps
 	drift();
@@ -365,17 +371,19 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD
 	else
 	{
 	  thetaPerSecond = trans.theta() / elapsedSeconds;
-	}    
+	}   
+   	float poseVarianceX, poseVarianceY;
+	getPoseVariances(50, poseVarianceX, poseVarianceY);	
     
-    m_LastUpdatePose = likeliestPose;
-    m_LastUpdateTime = measurementTime;
-    if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond )
+    if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond && poseVarianceX < 0.05 && poseVarianceY < 0.05 )
     {
       updateMap();
+	  m_LastUpdatePose = likeliestPose;
+	  m_LastUpdateTime = measurementTime;
     }
     else
     {
-      ROS_DEBUG_STREAM( "No mapping performed, rotation angle too big." );
+      ROS_WARN_STREAM( "No mapping performed, rotation angle too big." );
     }
   }
   else
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index c80692b22b0c147b9c4fbad1a0407681a3589ff4..0588db58fa5fe631ac087723dc03016ed494a910 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -113,34 +113,23 @@ void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
 void SlamNode::sendMapDataMessage(ros::Time mapTime)
 {
   std::vector<int8_t> mapData;
-  int width, height;
-  float resolution;
+  nav_msgs::MapMetaData metaData;
 
   OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
-  occMap->getOccupancyProbabilityImage (mapData, width, height, resolution);
+  occMap->getOccupancyProbabilityImage (mapData, metaData);
 
-  if ( width != height )
-  {
-    ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
-  }
-  else
+  //if ( width != height )
+  //{
+    //ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
+  //}
+  //else
   {
     nav_msgs::OccupancyGrid mapMsg;
     std_msgs::Header header;
     header.stamp = mapTime;
     header.frame_id = "map";
     mapMsg.header = header;
-    nav_msgs::MapMetaData mapMetaData;
-    mapMetaData.width = width;
-    mapMetaData.height = height;
-    mapMetaData.resolution = resolution;
-    mapMetaData.origin.position.x = -height*resolution/2;
-    mapMetaData.origin.position.y = -width*resolution/2;
-    mapMetaData.origin.orientation.w = 1.0;
-    mapMetaData.origin.orientation.x = 0.0;
-    mapMetaData.origin.orientation.y = 0.0;
-    mapMetaData.origin.orientation.z = 0.0;
-    mapMsg.info = mapMetaData;
+    mapMsg.info = metaData;
     mapMsg.data = mapData;
 
     m_SLAMMapPublisher.publish(mapMsg);
@@ -323,10 +312,10 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
     float res = msg->info.resolution;
     int height = msg->info.height; // cell size
     int width = msg->info.width; //cell size
-    if(height!=width) {
-        ROS_ERROR("Height != width in loaded map");
-        return;
-    }
+    //if(height!=width) {
+        //ROS_ERROR("Height != width in loaded map");
+        //return;
+    //}
 
     //convert map vector from ros format to robbie probability array
     float* map = new float[msg->data.size()];
@@ -359,7 +348,7 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
          }
      }
      Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY );
-     OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, exploredRegion);
+     OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, height, exploredRegion);
      m_HyperSlamFilter->setOccupancyMap( occMap );
      m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
      ROS_INFO_STREAM( "Replacing occupancy map" );
diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt
index 0d5bda2ddbb6e746e60c794acf7e937b5a698917..a763a4028feaa42a284eb30d82cf4558a628d7f0 100644
--- a/homer_navigation/CMakeLists.txt
+++ b/homer_navigation/CMakeLists.txt
@@ -16,7 +16,8 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Eigen3 REQUIRED)
 
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_BUILD_TYPE Debug)
 
 catkin_package(
   INCLUDE_DIRS include
diff --git a/homer_navigation/launch/homer_navigation.launch b/homer_navigation/launch/homer_navigation.launch
index db217818f217c26ead905d64d0bb2e7523d1a821..baa57eecae5383863adaae142ff65746babd7075 100644
--- a/homer_navigation/launch/homer_navigation.launch
+++ b/homer_navigation/launch/homer_navigation.launch
@@ -1,4 +1,4 @@
 <launch>
  <rosparam command="load" file="$(find homer_navigation)/config/homer_navigation.yaml"/>
- <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen"/>
+ <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen" launch-prefix="gdb"/> 
 </launch>
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 3c5d45c5c9968770d99a2ad9d9ecac2f7e23d439..c5de4ecce49f98698f240ff55adf9231a9a6560e 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -208,8 +208,6 @@ void HomerNavigationNode::calculatePath() {
     m_path_reaches_target = true;
     return;
   }
-  m_explorer->setOccupancyMap(m_width, m_height, m_origin,
-                              &(*m_last_map_data)[0]);
   m_explorer->setStart(
       map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
 
@@ -271,8 +269,6 @@ void HomerNavigationNode::startNavigation() {
     maskMap();
   }
 
-  m_explorer->setOccupancyMap(m_width, m_height, m_origin,
-                              &(*m_last_map_data)[0]);
 
   // check if there still exists a path to the original target
   if (m_avoided_collision && m_initial_path_reaches_target &&
@@ -865,7 +861,7 @@ void HomerNavigationNode::maskMap() {
   ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
                       safe_planning_box.max(), m_origin, m_resolution));
   for (size_t x = 0; x < m_width; x++) {
-    for (size_t y = 0; y < m_width; y++) {
+    for (size_t y = 0; y < m_height; y++){ 
       if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
         m_last_map_data->at(y * m_width + x) = -1;
       }
@@ -905,6 +901,8 @@ void HomerNavigationNode::mapCallback(
   m_width = msg->info.width;
   m_height = msg->info.height;
   m_resolution = msg->info.resolution;
+  m_explorer->setOccupancyMap(m_width, m_height, m_origin,
+                              &(*m_last_map_data)[0]);
 
   switch (m_MainMachine.state()) {
     case AWAITING_PATHPLANNING_MAP: