diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 9ccd73d1..0eb4d3d4 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -42,7 +42,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -60,7 +61,7 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				Eigen::Vector4f(0,0,100,0));
+				viewPoint);
 
 		if(segmentFlatObstacles)
 		{
@@ -155,7 +156,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -169,7 +171,8 @@ void segmentObstaclesFromGround(
 			minClusterSize,
 			segmentFlatObstacles,
 			maxGroundHeight,
-			flatObstacles);
+			flatObstacles,
+			viewPoint);
 }
 
 template<typename PointT>
diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h
index f3df87fb..9eca9d10 100644
--- a/corelib/include/rtabmap/core/util3d_mapping.h
+++ b/corelib/include/rtabmap/core/util3d_mapping.h
@@ -93,7 +93,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -105,7 +106,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
 
 template<typename PointT>
 void occupancy2DFromGroundObstacles(