diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 09cd2b02..32f57d28 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -255,8 +255,9 @@ void occupancy2DFromGroundObstacles(
 		ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
 		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
 		{
-			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
-			ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y;
+			cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
+			ptr[i][0] = groundCloudProjected->at(i).x;
+			ptr[i][1] = groundCloudProjected->at(i).y;
 		}
 	}
 
@@ -272,8 +273,9 @@ void occupancy2DFromGroundObstacles(
 		obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
 		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
 		{
-			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;
-			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y;
+			cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
+			ptr[i][0] = obstaclesCloudProjected->at(i).x;
+			ptr[i][1] = obstaclesCloudProjected->at(i).y;
 		}
 	}
 }
diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp
index 929e6c76..e2e90ce9 100644
--- a/corelib/src/OccupancyGrid.cpp
+++ b/corelib/src/OccupancyGrid.cpp
@@ -328,7 +328,6 @@ void OccupancyGrid::createLocalMap(
 					if(projRayTracing_)
 					{
 						cv::Mat laserScan = obstacles;
-						ground = cv::Mat();
 						obstacles = cv::Mat();
 						util3d::occupancy2DFromLaserScan(
 								laserScan,
diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp
index 5de1ed38..a675c76d 100644
--- a/corelib/src/util3d_mapping.cpp
+++ b/corelib/src/util3d_mapping.cpp
@@ -70,6 +70,23 @@ void occupancy2DFromLaserScan(
 	float xMin, yMin;
 	cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
 
+	// If input ground has already values, add them to map
+	if(ground.rows == 1 && ground.cols>0 && ground.type() == CV_32FC2)
+	{
+		for(int i=0; i<ground.cols; ++i)
+		{
+			cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
+
+			// cell
+			cv::Point2i cell((ptr[i][0]-xMin)/cellSize, (ptr[i][1]-yMin)/cellSize);
+
+			if(cell.x>=0 && cell.x<map8S.cols && cell.y >= 0 && cell.y < map8S.rows && map8S.at<char>(cell.y, cell.x) == -1)
+			{
+				map8S.at<char>(cell.y, cell.x) = 0;
+			}
+		}
+	}
+
 	// find ground cells
 	std::list<int> groundIndices;
 	for(unsigned int i=0; i< map8S.total(); ++i)
@@ -90,8 +107,9 @@ void occupancy2DFromLaserScan(
 		{
 			int y = *iter / map8S.cols;
 			int x = *iter - y*map8S.cols;
-			ground.at<cv::Vec2f>(i)[0] = (float(x))*cellSize + xMin;
-			ground.at<cv::Vec2f>(i)[1] = (float(y))*cellSize + yMin;
+			cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
+			ptr[i][0] = (float(x))*cellSize + xMin;
+			ptr[i][1] = (float(y))*cellSize + yMin;
 			++i;
 		}
 	}
@@ -103,8 +121,9 @@ void occupancy2DFromLaserScan(
 		obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
 		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
 		{
-			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
-			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
+			cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
+			ptr[i][0] = obstaclesCloud->at(i).x;
+			ptr[i][1] = obstaclesCloud->at(i).y;
 		}
 	}
 }
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 9a67f7c8..77a29453 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-491</y>
+            <y>-483</y>
             <width>673</width>
             <height>2649</height>
            </rect>
@@ -8860,7 +8860,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="4" column="1">
                           <widget class="QLabel" name="label_363">
                            <property name="text">
-                            <string>If 3D is not checked below, 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.</string>
+                            <string>2D ray tracing is done for each projected obstacle (when 3D is not checked below), filling unknown space between the sensor and obstacles.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>