diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index a7e2bf69bc4aef3b76f4219b5d5069596db6195f..64a5381c56c1abe8da38abddaa0f269c15664108 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -1,4 +1,4 @@
-/homer_mapping/size: 40                                          # size of one edge of the map in m. map is quadratic
+/homer_mapping/size: 4                                          # 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
 
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 0235e8604a89f3ea651bd3a1815eb393e0213cc3..20ade65d839d7e5211baee53a2121a082145ff78 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;
     
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 7318e38aaf7e129b1acb2d75445a503f81e4d994..3035a2061920e2c1d701d70cc68f0805e49d1e64 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -40,15 +40,16 @@ OccupancyMap::OccupancyMap()
   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)
 {
     initMembers();
 
 
-    m_Origin = origin;
-    m_Resolution = resolution;
-    m_PixelSize = pixelSize;
-    m_ByteSize = pixelSize * pixelSize;
+    m_metaData.origin = origin;
+    m_metaData.resolution = resolution;
+	m_metaData.width = width;
+	m_metaData.height = height;
+    m_ByteSize = m_metaData.width * m_metaData.height;
     m_ExploredRegion = exploredRegion;
     m_ChangedRegion = exploredRegion;
 
@@ -74,7 +75,6 @@ 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;
@@ -89,21 +89,23 @@ OccupancyMap::~OccupancyMap()
 
 void OccupancyMap::initMembers()
 {
-  float mapSize;
+  float mapSize, resolution;
   ros::param::get("/homer_mapping/size", mapSize);
-  ros::param::get("/homer_mapping/resolution", m_Resolution);
+  ros::param::get("/homer_mapping/resolution", 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;
+  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;
 
   ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
   ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
@@ -114,7 +116,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 +123,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
@@ -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,26 +171,82 @@ 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_metaData.width = new_width; 
+  m_metaData.height = new_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.position.x -= (x_add_left ) * m_metaData.resolution;
+  m_metaData.origin.position.y -= (y_add_up ) * m_metaData.resolution;
+
+  ROS_INFO_STREAM(" "<<m_metaData.origin.position);
+
+
+  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();
+  unsigned offset = m_metaData.width * p.y() + p.x();
   if ( offset > unsigned ( m_ByteSize ) )
   {
     return UNKNOWN_LIKELIHOOD;
@@ -209,7 +264,7 @@ 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;
@@ -279,7 +334,7 @@ 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 )
         {
@@ -359,93 +414,135 @@ void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time sta
   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);
+	  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_metaData.origin, m_metaData.resolution);
 
-      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;
-          }
-        }
-      }
-    }
+		  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++ )
   {
-      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 startPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+	  Eigen::Vector2i startPixel = map_tools::toMapCoords(startPosWorld, m_metaData.origin, m_metaData.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);
+	  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_PixelSize) endPixel.x() = m_PixelSize - 1;
-    if(endPixel.y() >= m_PixelSize) endPixel.y() = m_PixelSize - 1;
+	  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 );
+	  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 ( !ranges[i].free )
+	  {
+		  geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+		  Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
 
-      if ( endPixel != lastEndPixel )
-      {
-        unsigned offset = endPixel.x() + m_PixelSize * endPixel.y();
-        if ( offset < m_ByteSize )
-        {
-          m_CurrentChanges[ offset ] = ::OCCUPIED;
-        }
-      }
-      lastEndPixel=endPixel;
-      }
+		  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;
+			  }
+			  unsigned offset = endPixel.x() + m_metaData.width * 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);
-  
+  Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_metaData.origin, m_metaData.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);
+	  geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+	  Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
 
-    m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
-    m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
+	  m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
+	  m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
 
-    if ( endPixel != lastEndPixel )
-    {
-        drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
-    }
+	  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;
+
+	  if ( endPixel != lastEndPixel )
+	  {
+		  drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
+	  }
 
-    lastEndPixel=endPixel;
+	  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)
+  {
+	  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 +570,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;
@@ -528,7 +625,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 +676,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 +706,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 +724,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 +788,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;
@@ -723,7 +823,7 @@ void OccupancyMap::applyChanges()
 {
   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;
@@ -742,22 +842,22 @@ void OccupancyMap::applyChanges()
 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();
+  unsigned index = p.x() + m_metaData.width * p.y();
   if ( index < m_ByteSize )
   {
     if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
@@ -774,7 +874,7 @@ void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p )
 
 void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p )
 {
-  int index = p.x() + m_PixelSize * p.y();
+  int 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;
@@ -790,7 +890,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 +900,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,9 +916,9 @@ 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.3 / 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++ )
@@ -840,12 +934,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 +949,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 +970,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 +1048,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/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index c80692b22b0c147b9c4fbad1a0407681a3589ff4..d96d2cff9deceb7bbc4c6c1001a785f0dd754197 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);
@@ -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" );