61 bool segmentation_is_possible = initCompute ();
62 if (!segmentation_is_possible)
69 std::vector<float> height_thresholds;
70 std::vector<float> window_sizes;
72 float window_size = 0.0f;
73 float height_threshold = 0.0f;
75 while (window_size < max_window_size_)
79 window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
81 window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
85 height_threshold = initial_distance_;
87 height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
90 if (height_threshold > max_distance_)
91 height_threshold = max_distance_;
93 window_sizes.push_back (window_size);
94 height_thresholds.push_back (height_threshold);
104 for (std::size_t i = 0; i < window_sizes.size (); ++i)
106 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f)...",
107 i, height_thresholds[i], window_sizes[i]);
111 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
116 pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i],
MORPH_OPEN, *cloud_f);
121 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
123 float diff = (*cloud)[p_idx].z - (*cloud_f)[p_idx].z;
124 if (diff < height_thresholds[i])
125 pt_indices.push_back (ground[p_idx]);
129 ground.swap (pt_indices);
131 PCL_DEBUG (
"ground now has %d points\n", ground.size ());