diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index 6025d769c449fffcc88e050a47e41e51d24eed12..1ebc86225c33ce2f5edb4147a606045834a75576 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -53,7 +53,7 @@ gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the
 ## initial uncertainties
 gen.add("z_initial_std_dev", double_t, 0, "for ground reference initialization", 0.05, 0.01, 1.0);
 gen.add("initial_angular_std_dev_deg", double_t, 0, "for ground reference initialization", 1.5, 0.1, 10.0);
-## propagation additive noises
+## propagation X_k_additive noises
 gen.add("propagation_z_additive_noise_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.05, 0.0, 1.0);
 gen.add("propagation_additive_noise_deg_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.8, 0.0, 10.0);
 # measurement noise
@@ -74,6 +74,7 @@ gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with g
 
 # low obstacle detection parameters (requires a good 3d odometry source)
 gen.add("detect_low_obstacles_using_odometry", bool_t, 0, "to accumulate ground points and analyze them to find low obstacles", True);
+gen.add("ground_grid_size", double_t, 0, "ground grid will be of size 2*ground_grid_size squared", 10.0, 1.0, 50.0);
 gen.add("ground_grid_resolution", double_t, 0, "define the coarseness of the ground accumulation", 0.1, 0.01, 10.0);
 gen.add("number_of_neighbours_to_compare_height", int_t, 0, "a ground point will be declared low obstacle if the slope with any of its n neighbours is above a threshold", 4, 1, 100);
 gen.add("slope_threshold", double_t, 0, "Max ratio height change between ground points", 0.2, 0.01, 0.99);
diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index 1a3adb4b8cbe5c9afbc19648dc569d3619731664..72d52bf1f08f797268fe964abe97fe2c60162423 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/include/ground_segmentation_alg_node.h
@@ -31,6 +31,10 @@
 // [publisher subscriber headers]
 #include "ground_segmentation_alg.h"
 #include <nav_msgs/Odometry.h>
+#include <tf/tf.h>
+#include <pcl/common/transforms.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <pcl/filters/passthrough.h>
 
 // [service client headers]
 
@@ -44,6 +48,7 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<Ground
 {
 private:
   bool detect_low_obstacles_using_odometry_;
+  float ground_grid_size_;
   float ground_grid_resolution_;
   int number_of_neighbours_to_compare_height_;
   float slope_threshold_;
@@ -88,7 +93,8 @@ private:
   nav_msgs::Odometry odom_3d_;
   void cbGetOdomMsg(const nav_msgs::Odometry::ConstPtr &msg);
 
-  pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation_;
+  pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation_in_odom_frame_;
+  Eigen::Matrix<float, 6, 1> X_k_;           // x, y, z, roll, pitch, yaw in current iteration
 
   pthread_mutex_t mutex_;
   void mutex_enter(void);
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 9c58f269d0216dc0b7d3561b12df92eef53220fa..4d0f2dfebf29ba37e3e95c5403fb06e3a6779cc8 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -170,6 +170,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   else
     this->detect_low_obstacles_using_odometry_ = config_.detect_low_obstacles_using_odometry;
 
+  if (!this->private_node_handle_.getParam("ground_grid_size", this->config_.ground_grid_size))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_grid_size' not found");
+  }
+  else
+    this->ground_grid_size_ = config_.ground_grid_size;
+
   if (!this->private_node_handle_.getParam("ground_grid_resolution", this->config_.ground_grid_resolution))
   {
     ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_grid_resolution' not found");
@@ -431,8 +438,127 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
   std::cout << "-------------------------------------------------------------------------------" << std::endl;
 }
 
+void transformPointCloud(const Eigen::Matrix<float, 6, 1> pose,
+                         const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud,
+                         pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud)
+{
+  Eigen::Matrix<float, 4, 4> rigid_transform = Eigen::Matrix<float, 4, 4>::Identity();
+
+  rigid_transform(0, 3) = pose(0, 0);
+  rigid_transform(1, 3) = pose(1, 0);
+  rigid_transform(2, 3) = pose(2, 0);
+
+  tf::Matrix3x3 rot;
+  rot.setRPY(pose(3, 0), pose(4, 0), pose(5, 0));
+
+  tf::Vector3 x_row = rot.getRow(0);
+  tf::Vector3 y_row = rot.getRow(1);
+  tf::Vector3 z_row = rot.getRow(2);
+
+  rigid_transform(0, 0) = x_row.getX();
+  rigid_transform(0, 1) = x_row.getY();
+  rigid_transform(0, 2) = x_row.getZ();
+
+  rigid_transform(1, 0) = y_row.getX();
+  rigid_transform(1, 1) = y_row.getY();
+  rigid_transform(1, 2) = y_row.getZ();
+
+  rigid_transform(2, 0) = z_row.getX();
+  rigid_transform(2, 1) = z_row.getY();
+  rigid_transform(2, 2) = z_row.getZ();
+
+  //std::cout << "Transformation = " << std::endl << rigid_transform << std::endl;
+
+  pcl::transformPointCloud(input_pcl_cloud, output_pcl_cloud, rigid_transform);
+}
+
+void transformInversePointCloud(const Eigen::Matrix<float, 6, 1> pose,
+                                 const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud,
+                                 pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud)
+{
+  Eigen::Matrix<float, 4, 4> rigid_transform = Eigen::Matrix<float, 4, 4>::Identity();
+
+  rigid_transform(0, 3) = pose(0, 0);
+  rigid_transform(1, 3) = pose(1, 0);
+  rigid_transform(2, 3) = pose(2, 0);
+
+  tf::Matrix3x3 rot;
+  rot.setRPY(pose(3, 0), pose(4, 0), pose(5, 0));
+
+  tf::Vector3 x_row = rot.getRow(0);
+  tf::Vector3 y_row = rot.getRow(1);
+  tf::Vector3 z_row = rot.getRow(2);
+
+  rigid_transform(0, 0) = x_row.getX();
+  rigid_transform(0, 1) = x_row.getY();
+  rigid_transform(0, 2) = x_row.getZ();
+
+  rigid_transform(1, 0) = y_row.getX();
+  rigid_transform(1, 1) = y_row.getY();
+  rigid_transform(1, 2) = y_row.getZ();
+
+  rigid_transform(2, 0) = z_row.getX();
+  rigid_transform(2, 1) = z_row.getY();
+  rigid_transform(2, 2) = z_row.getZ();
+
+  rigid_transform = rigid_transform.inverse();
+
+  //std::cout << "Transformation = " << std::endl << rigid_transform << std::endl;
+
+  pcl::transformPointCloud(input_pcl_cloud, output_pcl_cloud, rigid_transform);
+}
+
+void discardPointsOutOfROI(const Eigen::Matrix<float, 6, 1> pose, const float ground_grid_size, pcl::PointCloud<pcl::PointXYZRGBNormal> &point_cloud_in_pose_frame)
+{
+  pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_in_local_frame;
+  transformInversePointCloud(pose, point_cloud_in_pose_frame, point_cloud_in_local_frame);
+
+  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr point_cloud_in_local_frame_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+  *point_cloud_in_local_frame_ptr = point_cloud_in_local_frame;
+
+  pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
+  pass.setInputCloud (point_cloud_in_local_frame_ptr);
+  pass.setFilterFieldName ("x");
+  pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size);
+  pass.filter (*point_cloud_in_local_frame_ptr);
+
+  pass.setInputCloud (point_cloud_in_local_frame_ptr);
+  pass.setFilterFieldName ("y");
+  pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size);
+  pass.filter (*point_cloud_in_local_frame_ptr);
+
+  point_cloud_in_local_frame.clear();
+  point_cloud_in_local_frame = *point_cloud_in_local_frame_ptr;
+
+  transformPointCloud(pose, point_cloud_in_local_frame, point_cloud_in_pose_frame);
+}
+
 void GroundSegmentationAlgNode::detectLowObstacles(pcl::PointCloud<pcl::PointXYZRGBNormal> &lidar_pcl_cloud)
 {
+  pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points;
+  for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud.begin(); i != lidar_pcl_cloud.end(); ++i)
+    if((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
+      ground_points.push_back(*i);
+
+  pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_in_odom_frame;
+
+  transformPointCloud(X_k_, ground_points, ground_points_in_odom_frame);
+
+  ground_points_accumulation_in_odom_frame_ += ground_points_in_odom_frame;
+
+  discardPointsOutOfROI(X_k_, ground_grid_size_, ground_points_accumulation_in_odom_frame_);
+
+  pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation;
+  transformInversePointCloud(X_k_, ground_points_accumulation_in_odom_frame_, ground_points_accumulation);
+
+  for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = ground_points_accumulation.begin(); i != ground_points_accumulation.end(); ++i)
+  {
+    i->r = R_CLASS_GROUND;
+    i->g = G_CLASS_GROUND;
+    i->b = B_CLASS_GROUND;
+
+    lidar_pcl_cloud.push_back(*i);
+  }
 
 }
 
@@ -450,10 +576,11 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
     this->alg_.lock();
     this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_,
                              ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
+    this->alg_.unlock();
 
+    this->mutex_enter(); // Because we are going to use the odometry information, and we do not want the callback to collide with the processing
     this->detectLowObstacles(lidar_pcl_cloud_);
-
-    this->alg_.unlock();
+    this->mutex_exit();
 
     this->convertOutputsToSuitableFormats();
     this->publishAll();
@@ -480,6 +607,22 @@ void GroundSegmentationAlgNode::cbGetOdomMsg(const nav_msgs::Odometry::ConstPtr
   this->mutex_enter();
   //std::cout << "GroundSegmentationAlgNode::cbGetOdomMsg --> odom message received!" << std::endl;
   odom_3d_ = *msg;
+
+  X_k_(0) = odom_3d_.pose.pose.position.x;
+  X_k_(1) = odom_3d_.pose.pose.position.y;
+  X_k_(2) = odom_3d_.pose.pose.position.z;
+
+  double roll, pitch, yaw;
+  tf2::Quaternion q_pose(odom_3d_.pose.pose.orientation.x, odom_3d_.pose.pose.orientation.y,
+                         odom_3d_.pose.pose.orientation.z, odom_3d_.pose.pose.orientation.w);
+
+  tf2::Matrix3x3 m_pose(q_pose);
+  m_pose.getRPY(roll, pitch, yaw);
+
+  X_k_(3) = roll;  //X_k_minus_one_(3, 0);
+  X_k_(4) = pitch; //X_k_minus_one_(4, 0);
+  X_k_(5) = yaw;
+
   this->mutex_exit();
 }