Point Cloud Library (PCL)  1.7.1
person_cluster.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * person_cluster.hpp
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #ifndef PCL_PEOPLE_PERSON_CLUSTER_HPP_
42 #define PCL_PEOPLE_PERSON_CLUSTER_HPP_
43 
44 #include <pcl/people/person_cluster.h>
45 
46 template <typename PointT>
48  const PointCloudPtr& input_cloud,
49  const pcl::PointIndices& indices,
50  const Eigen::VectorXf& ground_coeffs,
51  float sqrt_ground_coeffs,
52  bool head_centroid,
53  bool vertical)
54  {
55  init(input_cloud, indices, ground_coeffs, sqrt_ground_coeffs, head_centroid, vertical);
56  }
57 
58 template <typename PointT> void
60  const PointCloudPtr& input_cloud,
61  const pcl::PointIndices& indices,
62  const Eigen::VectorXf& ground_coeffs,
63  float sqrt_ground_coeffs,
64  bool head_centroid,
65  bool vertical)
66 {
67 
68  vertical_ = vertical;
69  head_centroid_ = head_centroid;
70  person_confidence_ = std::numeric_limits<float>::quiet_NaN();
71 
72  min_x_ = 1000.0f;
73  min_y_ = 1000.0f;
74  min_z_ = 1000.0f;
75 
76  max_x_ = -1000.0f;
77  max_y_ = -1000.0f;
78  max_z_ = -1000.0f;
79 
80  sum_x_ = 0.0f;
81  sum_y_ = 0.0f;
82  sum_z_ = 0.0f;
83 
84  n_ = 0;
85 
86  points_indices_.indices = indices.indices;
87 
88  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
89  {
90  PointT* p = &input_cloud->points[*pit];
91 
92  min_x_ = std::min(p->x, min_x_);
93  max_x_ = std::max(p->x, max_x_);
94  sum_x_ += p->x;
95 
96  min_y_ = std::min(p->y, min_y_);
97  max_y_ = std::max(p->y, max_y_);
98  sum_y_ += p->y;
99 
100  min_z_ = std::min(p->z, min_z_);
101  max_z_ = std::max(p->z, max_z_);
102  sum_z_ += p->z;
103 
104  n_++;
105  }
106 
107  c_x_ = sum_x_ / n_;
108  c_y_ = sum_y_ / n_;
109  c_z_ = sum_z_ / n_;
110 
111 
112  Eigen::Vector4f height_point(c_x_, c_y_, c_z_, 1.0f);
113  if(!vertical_)
114  {
115  height_point(1) = min_y_;
116  distance_ = std::sqrt(c_x_ * c_x_ + c_z_ * c_z_);
117  }
118  else
119  {
120  height_point(0) = max_x_;
121  distance_ = std::sqrt(c_y_ * c_y_ + c_z_ * c_z_);
122  }
123 
124  float height = std::fabs(height_point.dot(ground_coeffs));
125  height /= sqrt_ground_coeffs;
126  height_ = height;
127 
128  if(head_centroid_)
129  {
130  float sum_x = 0.0f;
131  float sum_y = 0.0f;
132  float sum_z = 0.0f;
133  int n = 0;
134 
135  float head_threshold_value; // vertical coordinate of the lowest head point
136  if (!vertical_)
137  {
138  head_threshold_value = min_y_ + height_ / 8.0f; // head is suppose to be 1/8 of the human height
139  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
140  {
141  PointT* p = &input_cloud->points[*pit];
142 
143  if(p->y < head_threshold_value)
144  {
145  sum_x += p->x;
146  sum_y += p->y;
147  sum_z += p->z;
148  n++;
149  }
150  }
151  }
152  else
153  {
154  head_threshold_value = max_x_ - height_ / 8.0f; // head is suppose to be 1/8 of the human height
155  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
156  {
157  PointT* p = &input_cloud->points[*pit];
158 
159  if(p->x > head_threshold_value)
160  {
161  sum_x += p->x;
162  sum_y += p->y;
163  sum_z += p->z;
164  n++;
165  }
166  }
167  }
168 
169  c_x_ = sum_x / n;
170  c_y_ = sum_y / n;
171  c_z_ = sum_z / n;
172  }
173 
174  if(!vertical_)
175  {
176  float min_x = c_x_;
177  float min_z = c_z_;
178  float max_x = c_x_;
179  float max_z = c_z_;
180  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
181  {
182  PointT* p = &input_cloud->points[*pit];
183 
184  min_x = std::min(p->x, min_x);
185  max_x = std::max(p->x, max_x);
186  min_z = std::min(p->z, min_z);
187  max_z = std::max(p->z, max_z);
188  }
189 
190  angle_ = std::atan2(c_z_, c_x_);
191  angle_max_ = std::max(std::atan2(min_z, min_x), std::atan2(max_z, min_x));
192  angle_min_ = std::min(std::atan2(min_z, max_x), std::atan2(max_z, max_x));
193 
194  Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f);
195  float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2);
196  float bottom_x = c_x_ - ground_coeffs(0) * t;
197  float bottom_y = c_y_ - ground_coeffs(1) * t;
198  float bottom_z = c_z_ - ground_coeffs(2) * t;
199 
200  tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
201  Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;
202 
203  ttop_ = v * height / v.norm() + tbottom_;
204  tcenter_ = v * height * 0.5 / v.norm() + tbottom_;
205  top_ = Eigen::Vector3f(c_x_, min_y_, c_z_);
206  bottom_ = Eigen::Vector3f(c_x_, max_y_, c_z_);
207  center_ = Eigen::Vector3f(c_x_, c_y_, c_z_);
208 
209  min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);
210 
211  max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
212  }
213  else
214  {
215  float min_y = c_y_;
216  float min_z = c_z_;
217  float max_y = c_y_;
218  float max_z = c_z_;
219  for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
220  {
221  PointT* p = &input_cloud->points[*pit];
222 
223  min_y = std::min(p->y, min_y);
224  max_y = std::max(p->y, max_y);
225  min_z = std::min(p->z, min_z);
226  max_z = std::max(p->z, max_z);
227  }
228 
229  angle_ = std::atan2(c_z_, c_y_);
230  angle_max_ = std::max(std::atan2(min_z_, min_y_), std::atan2(max_z_, min_y_));
231  angle_min_ = std::min(std::atan2(min_z_, max_y_), std::atan2(max_z_, max_y_));
232 
233  Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f);
234  float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2);
235  float bottom_x = c_x_ - ground_coeffs(0) * t;
236  float bottom_y = c_y_ - ground_coeffs(1) * t;
237  float bottom_z = c_z_ - ground_coeffs(2) * t;
238 
239  tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
240  Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;
241 
242  ttop_ = v * height / v.norm() + tbottom_;
243  tcenter_ = v * height * 0.5 / v.norm() + tbottom_;
244  top_ = Eigen::Vector3f(max_x_, c_y_, c_z_);
245  bottom_ = Eigen::Vector3f(min_x_, c_y_, c_z_);
246  center_ = Eigen::Vector3f(c_x_, c_y_, c_z_);
247 
248  min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);
249 
250  max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
251  }
252 }
253 
254 template <typename PointT> pcl::PointIndices&
256 {
257  return (points_indices_);
258 }
259 
260 template <typename PointT> float
262 {
263  return (height_);
264 }
265 
266 template <typename PointT> float
267 pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs)
268 {
269  float sqrt_ground_coeffs = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
270  return (updateHeight(ground_coeffs, sqrt_ground_coeffs));
271 }
272 
273 template <typename PointT> float
274 pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs, float sqrt_ground_coeffs)
275 {
276  Eigen::Vector4f height_point;
277  if (!vertical_)
278  height_point << c_x_, min_y_, c_z_, 1.0f;
279  else
280  height_point << max_x_, c_y_, c_z_, 1.0f;
281 
282  float height = std::fabs(height_point.dot(ground_coeffs));
283  height /= sqrt_ground_coeffs;
284  height_ = height;
285  return (height_);
286 }
287 
288 template <typename PointT> float
290 {
291  return (distance_);
292 }
293 
294 template <typename PointT> Eigen::Vector3f&
296 {
297  return (ttop_);
298 }
299 
300 template <typename PointT> Eigen::Vector3f&
302 {
303  return (tbottom_);
304 }
305 
306 template <typename PointT> Eigen::Vector3f&
308 {
309  return (tcenter_);
310 }
311 
312 template <typename PointT> Eigen::Vector3f&
314 {
315  return (top_);
316 }
317 
318 template <typename PointT> Eigen::Vector3f&
320 {
321  return (bottom_);
322 }
323 
324 template <typename PointT> Eigen::Vector3f&
326 {
327  return (center_);
328 }
329 
330 template <typename PointT> Eigen::Vector3f&
332 {
333  return (min_);
334 }
335 
336 template <typename PointT> Eigen::Vector3f&
338 {
339  return (max_);
340 }
341 
342 template <typename PointT> float
344 {
345  return (angle_);
346 }
347 
348 template <typename PointT>
350 {
351  return (angle_max_);
352 }
353 
354 template <typename PointT>
356 {
357  return (angle_min_);
358 }
359 
360 template <typename PointT>
362 {
363  return (n_);
364 }
365 
366 template <typename PointT>
368 {
369  return (person_confidence_);
370 }
371 
372 template <typename PointT>
374 {
375  person_confidence_ = confidence;
376 }
377 
378 template <typename PointT>
380 {
381  height_ = height;
382 }
383 
384 template <typename PointT>
386 {
387  // draw theoretical person bounding box in the PCL viewer:
388  pcl::ModelCoefficients coeffs;
389  // translation
390  coeffs.values.push_back (tcenter_[0]);
391  coeffs.values.push_back (tcenter_[1]);
392  coeffs.values.push_back (tcenter_[2]);
393  // rotation
394  coeffs.values.push_back (0.0);
395  coeffs.values.push_back (0.0);
396  coeffs.values.push_back (0.0);
397  coeffs.values.push_back (1.0);
398  // size
399  if (vertical_)
400  {
401  coeffs.values.push_back (height_);
402  coeffs.values.push_back (0.5);
403  coeffs.values.push_back (0.5);
404  }
405  else
406  {
407  coeffs.values.push_back (0.5);
408  coeffs.values.push_back (height_);
409  coeffs.values.push_back (0.5);
410  }
411 
412  std::stringstream bbox_name;
413  bbox_name << "bbox_person_" << person_number;
414  viewer.removeShape (bbox_name.str());
415  viewer.addCube (coeffs, bbox_name.str());
416  viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name.str());
418 
419  // std::stringstream confid;
420  // confid << person_confidence_;
421  // PointT position;
422  // position.x = tcenter_[0]- 0.2;
423  // position.y = ttop_[1];
424  // position.z = tcenter_[2];
425  // viewer.addText3D(confid.str().substr(0, 4), position, 0.1);
426 }
427 
428 template <typename PointT>
430 {
431  // Auto-generated destructor stub
432 }
433 #endif /* PCL_PEOPLE_PERSON_CLUSTER_HPP_ */