70 bool segmentation_is_possible = initCompute ();
71 if (!segmentation_is_possible)
78 std::vector<float> height_thresholds;
79 std::vector<float> window_sizes;
81 float window_size = 0.0f;
82 float height_threshold = 0.0f;
84 while (window_size < max_window_size_)
88 window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
90 window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
94 height_threshold = initial_distance_;
96 height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
99 if (height_threshold > max_distance_)
100 height_threshold = max_distance_;
102 window_sizes.push_back (window_size);
103 height_thresholds.push_back (height_threshold);
113 for (std::size_t i = 0; i < window_sizes.size (); ++i)
115 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f)...",
116 i, height_thresholds[i], window_sizes[i]);
120 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
125 pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i],
MORPH_OPEN, *cloud_f);
130 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
132 float diff = (*cloud)[p_idx].z - (*cloud_f)[p_idx].z;
133 if (diff < height_thresholds[i])
134 pt_indices.push_back (ground[p_idx]);
138 ground.swap (pt_indices);
140 PCL_DEBUG (
"ground now has %d points\n", ground.size ());