41 #ifndef PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
42 #define PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
44 #include <pcl/people/ground_based_people_detection_app.h>
46 template <
typename Po
intT>
55 head_centroid_ =
true;
60 dimension_limits_set_ =
false;
61 heads_minimum_distance_ = 0.3;
64 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
65 person_classifier_set_flag_ =
false;
68 template <
typename Po
intT>
void
74 template <
typename Po
intT>
void
77 ground_coeffs_ = ground_coeffs;
78 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
81 template <
typename Po
intT>
void
84 sampling_factor_ = sampling_factor;
87 template <
typename Po
intT>
void
90 voxel_size_ = voxel_size;
93 template <
typename Po
intT>
void
96 intrinsics_matrix_ = intrinsics_matrix;
99 template <
typename Po
intT>
void
102 person_classifier_ = person_classifier;
103 person_classifier_set_flag_ =
true;
106 template <
typename Po
intT>
void
109 vertical_ = vertical;
112 template <
typename Po
intT>
void
115 min_height_ = min_height;
116 max_height_ = max_height;
119 template <
typename Po
intT>
void
122 min_points_ = min_points;
123 max_points_ = max_points;
124 dimension_limits_set_ =
true;
127 template <
typename Po
intT>
void
130 heads_minimum_distance_= heads_minimum_distance;
133 template <
typename Po
intT>
void
136 head_centroid_ = head_centroid;
139 template <
typename Po
intT>
void
142 min_height = min_height_;
143 max_height = max_height_;
146 template <
typename Po
intT>
void
149 min_points = min_points_;
150 max_points = max_points_;
153 template <
typename Po
intT>
float
156 return (heads_minimum_distance_);
159 template <
typename Po
intT> Eigen::VectorXf
162 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
164 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
166 return (ground_coeffs_);
172 return (no_ground_cloud_);
175 template <
typename Po
intT>
void
179 output_cloud->
points.resize(input_cloud->height*input_cloud->width);
180 output_cloud->
width = input_cloud->width;
181 output_cloud->
height = input_cloud->height;
184 for (
int j = 0; j < input_cloud->width; j++)
186 for (
int i = 0; i < input_cloud->height; i++)
188 rgb_point.r = (*input_cloud)(j,i).r;
189 rgb_point.g = (*input_cloud)(j,i).g;
190 rgb_point.b = (*input_cloud)(j,i).b;
191 (*output_cloud)(j,i) = rgb_point;
196 template <
typename Po
intT>
void
203 for (
int i = 0; i < cloud->
width; i++)
205 for (
int j = 0; j < cloud->
height; j++)
207 (*output_cloud)(j,i) = (*cloud)(cloud->
width - i - 1, j);
210 cloud = output_cloud;
213 template <
typename Po
intT>
bool
217 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
219 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
224 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
227 if (intrinsics_matrix_(0) == 0)
229 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
232 if (!person_classifier_set_flag_)
234 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
238 if (!dimension_limits_set_)
241 max_points_ = int(
float(max_points_) * std::pow(0.06/voxel_size_, 2));
242 if (voxel_size_ > 0.06)
243 min_points_ = int(
float(min_points_) * std::pow(0.06/voxel_size_, 2));
247 rgb_image_->points.clear();
248 extractRGBFromPointCloud(cloud_, rgb_image_);
251 if (sampling_factor_ != 1)
254 cloud_downsampled->width = (cloud_->width)/sampling_factor_;
255 cloud_downsampled->height = (cloud_->height)/sampling_factor_;
256 cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
257 cloud_downsampled->is_dense = cloud_->is_dense;
258 for (
int j = 0; j < cloud_downsampled->width; j++)
260 for (
int i = 0; i < cloud_downsampled->height; i++)
262 (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
265 (*cloud_) = (*cloud_downsampled);
272 voxel_grid_filter_object.
setLeafSize (voxel_size_, voxel_size_, voxel_size_);
273 voxel_grid_filter_object.
filter (*cloud_filtered);
278 ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
284 extract.
filter(*no_ground_cloud_);
285 if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
286 ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_);
288 PCL_INFO (
"No groundplane update!\n");
291 std::vector<pcl::PointIndices> cluster_indices;
315 swapDimensions(rgb_image_);
320 Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter());
321 centroid /= centroid(2);
322 Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop());
324 Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom());
326 it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
332 template <
typename Po
intT>