diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 521f2a33786febd4e0dc60653ddc53bb95909926..d8ed96ff96c20ecc88cfc979a618584ce8750da1 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -846,7 +846,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
     std::vector<int> pointIdxKNNSearch(K);
     std::vector<float> pointKNNSquaredDistance(K);
 
-    kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch, pointKNNSquaredDistance);
+    int num_of_references_found = kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch, pointKNNSquaredDistance);
 
     int best_reference_index = -1;
     int best_reference_index_predicting_ground = -1;
@@ -856,7 +856,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
     float min_obstacle_pred_std_dev = 10000.0;
     float best_pred_std_dev = 10000.0;
 
-    for (int reference_iterator = 0; reference_iterator < K; ++reference_iterator)
+    for (int reference_iterator = 0; reference_iterator < num_of_references_found; ++reference_iterator)
     {
       pcl::PointXYZRGBNormal reference_in_sensor_frame =
           ground_reference_cloud_ptr->points[pointIdxKNNSearch[reference_iterator]];