Point Cloud Library (PCL)  1.9.0-dev
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_ */
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
int getNumberPoints()
Returns the number of points of the cluster.
PersonCluster(const PointCloudPtr &input_cloud, const pcl::PointIndices &indices, const Eigen::VectorXf &ground_coeffs, float sqrt_ground_coeffs, bool head_centroid, bool vertical=false)
Constructor.
float getHeight()
Returns the height of the cluster.
std::vector< float > values
Eigen::Vector3f & getCenter()
Returns the centroid.
Eigen::Vector3f & getTCenter()
Returns the theoretical centroid (at half height).
std::vector< int > indices
Definition: PointIndices.h:19
float getAngle()
Returns the angle formed by the cluster&#39;s centroid with respect to the sensor (in radians)...
void init(const PointCloudPtr &input_cloud, const pcl::PointIndices &indices, const Eigen::VectorXf &ground_coeffs, float sqrt_ground_coeffs, bool head_centroid, bool vertical)
PersonCluster initialization.
float getAngleMin()
Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
Eigen::Vector3f & getTTop()
Returns the theoretical top point.
Eigen::Vector3f & getBottom()
Returns the bottom point.
3 floats (R, G, B) going from 0.0 (dark) to 1.0 (light)
Definition: common.h:103
PCL Visualizer main class.
bool addCube(const pcl::ModelCoefficients &coefficients, const std::string &id="cube", int viewport=0)
Add a cube from a set of given model coefficients.
Eigen::Vector3f & getTop()
Returns the top point.
boost::shared_ptr< PointCloud > PointCloudPtr
virtual ~PersonCluster()
Destructor.
float getAngleMax()
Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
float getPersonConfidence()
Returns the HOG confidence.
void setPersonConfidence(float confidence)
Sets the HOG confidence.
void drawTBoundingBox(pcl::visualization::PCLVisualizer &viewer, int person_number)
Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
float getDistance()
Returns the distance of the cluster from the sensor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
bool removeShape(const std::string &id="cloud", int viewport=0)
Removes an added shape from screen (line, polygon, etc.), based on a given ID.
bool setShapeRenderingProperties(int property, double value, const std::string &id, int viewport=0)
Set the rendering properties of a shape.
float updateHeight(const Eigen::VectorXf &ground_coeffs)
Update the height of the cluster.
void setHeight(float height)
Sets the cluster height.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
Eigen::Vector3f & getTBottom()
Returns the theoretical bottom point.