Point Cloud Library (PCL)  1.7.1
person_cluster.h
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.h
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #ifndef PCL_PEOPLE_PERSON_CLUSTER_H_
42 #define PCL_PEOPLE_PERSON_CLUSTER_H_
43 
44 #include <pcl/point_types.h>
45 #include <pcl/visualization/pcl_visualizer.h>
46 
47 namespace pcl
48 {
49  namespace people
50  {
51  /** \brief @b PersonCluster represents a class for representing information about a cluster containing a person.
52  * \author Filippo Basso, Matteo Munaro
53  * \ingroup people
54  */
55  template <typename PointT> class PersonCluster;
56  template <typename PointT> bool operator<(const PersonCluster<PointT>& c1, const PersonCluster<PointT>& c2);
57 
58  template <typename PointT>
59  class PersonCluster
60  {
61  protected:
62 
64 
65  /** \brief Minimum x coordinate of the cluster points. */
66  float min_x_;
67  /** \brief Minimum y coordinate of the cluster points. */
68  float min_y_;
69  /** \brief Minimum z coordinate of the cluster points. */
70  float min_z_;
71 
72  /** \brief Maximum x coordinate of the cluster points. */
73  float max_x_;
74  /** \brief Maximum y coordinate of the cluster points. */
75  float max_y_;
76  /** \brief Maximum z coordinate of the cluster points. */
77  float max_z_;
78 
79  /** \brief Sum of x coordinates of the cluster points. */
80  float sum_x_;
81  /** \brief Sum of y coordinates of the cluster points. */
82  float sum_y_;
83  /** \brief Sum of z coordinates of the cluster points. */
84  float sum_z_;
85 
86  /** \brief Number of cluster points. */
87  int n_;
88 
89  /** \brief x coordinate of the cluster centroid. */
90  float c_x_;
91  /** \brief y coordinate of the cluster centroid. */
92  float c_y_;
93  /** \brief z coordinate of the cluster centroid. */
94  float c_z_;
95 
96  /** \brief Cluster height from the ground plane. */
97  float height_;
98 
99  /** \brief Cluster distance from the sensor. */
100  float distance_;
101  /** \brief Cluster centroid horizontal angle with respect to z axis. */
102  float angle_;
103 
104  /** \brief Maximum angle of the cluster points. */
105  float angle_max_;
106  /** \brief Minimum angle of the cluster points. */
107  float angle_min_;
108 
109  /** \brief Cluster top point. */
110  Eigen::Vector3f top_;
111  /** \brief Cluster bottom point. */
112  Eigen::Vector3f bottom_;
113  /** \brief Cluster centroid. */
114  Eigen::Vector3f center_;
115 
116  /** \brief Theoretical cluster top. */
117  Eigen::Vector3f ttop_;
118  /** \brief Theoretical cluster bottom (lying on the ground plane). */
119  Eigen::Vector3f tbottom_;
120  /** \brief Theoretical cluster center (between ttop_ and tbottom_). */
121  Eigen::Vector3f tcenter_;
122 
123  /** \brief Vector containing the minimum coordinates of the cluster. */
124  Eigen::Vector3f min_;
125  /** \brief Vector containing the maximum coordinates of the cluster. */
126  Eigen::Vector3f max_;
127 
128  /** \brief Point cloud indices of the cluster points. */
130 
131  /** \brief If true, the sensor is considered to be vertically placed (portrait mode). */
132  bool vertical_;
133  /** \brief PersonCluster HOG confidence. */
135 
136  public:
137 
139  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
140  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
141 
142  /** \brief Constructor. */
143  PersonCluster (
144  const PointCloudPtr& input_cloud,
145  const pcl::PointIndices& indices,
146  const Eigen::VectorXf& ground_coeffs,
147  float sqrt_ground_coeffs,
148  bool head_centroid,
149  bool vertical = false);
150 
151  /** \brief Destructor. */
152  virtual ~PersonCluster ();
153 
154  /**
155  * \brief Returns the height of the cluster.
156  * \return the height of the cluster.
157  */
158  float
159  getHeight ();
160 
161  /**
162  * \brief Update the height of the cluster.
163  * \param[in] ground_coeffs The coefficients of the ground plane.
164  * \return the height of the cluster.
165  */
166  float
167  updateHeight (const Eigen::VectorXf& ground_coeffs);
168 
169  /**
170  * \brief Update the height of the cluster.
171  * \param[in] ground_coeffs The coefficients of the ground plane.
172  * \param[in] sqrt_ground_coeffs The norm of the vector [a, b, c] where a, b and c are the first
173  * three coefficients of the ground plane (ax + by + cz + d = 0).
174  * \return the height of the cluster.
175  */
176  float
177  updateHeight (const Eigen::VectorXf& ground_coeffs, float sqrt_ground_coeffs);
178 
179  /**
180  * \brief Returns the distance of the cluster from the sensor.
181  * \return the distance of the cluster (its centroid) from the sensor without considering the
182  * y dimension.
183  */
184  float
185  getDistance ();
186 
187  /**
188  * \brief Returns the angle formed by the cluster's centroid with respect to the sensor (in radians).
189  * \return the angle formed by the cluster's centroid with respect to the sensor (in radians).
190  */
191  float
192  getAngle ();
193 
194  /**
195  * \brief Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
196  * \return the minimum angle formed by the cluster with respect to the sensor (in radians).
197  */
198  float
199  getAngleMin ();
200 
201  /**
202  * \brief Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
203  * \return the maximum angle formed by the cluster with respect to the sensor (in radians).
204  */
205  float
206  getAngleMax ();
207 
208  /**
209  * \brief Returns the indices of the point cloud points corresponding to the cluster.
210  * \return the indices of the point cloud points corresponding to the cluster.
211  */
213  getIndices ();
214 
215  /**
216  * \brief Returns the theoretical top point.
217  * \return the theoretical top point.
218  */
219  Eigen::Vector3f&
220  getTTop ();
221 
222  /**
223  * \brief Returns the theoretical bottom point.
224  * \return the theoretical bottom point.
225  */
226  Eigen::Vector3f&
227  getTBottom ();
228 
229  /**
230  * \brief Returns the theoretical centroid (at half height).
231  * \return the theoretical centroid (at half height).
232  */
233  Eigen::Vector3f&
234  getTCenter ();
235 
236  /**
237  * \brief Returns the top point.
238  * \return the top point.
239  */
240  Eigen::Vector3f&
241  getTop ();
242 
243  /**
244  * \brief Returns the bottom point.
245  * \return the bottom point.
246  */
247  Eigen::Vector3f&
248  getBottom ();
249 
250  /**
251  * \brief Returns the centroid.
252  * \return the centroid.
253  */
254  Eigen::Vector3f&
255  getCenter ();
256 
257  //Eigen::Vector3f& getTMax();
258 
259  /**
260  * \brief Returns the point formed by min x, min y and min z.
261  * \return the point formed by min x, min y and min z.
262  */
263  Eigen::Vector3f&
264  getMin ();
265 
266  /**
267  * \brief Returns the point formed by max x, max y and max z.
268  * \return the point formed by max x, max y and max z.
269  */
270  Eigen::Vector3f&
271  getMax ();
272 
273  /**
274  * \brief Returns the HOG confidence.
275  * \return the HOG confidence.
276  */
277  float
279 
280  /**
281  * \brief Returns the number of points of the cluster.
282  * \return the number of points of the cluster.
283  */
284  int
285  getNumberPoints ();
286 
287  /**
288  * \brief Sets the cluster height.
289  * \param[in] height
290  */
291  void
292  setHeight (float height);
293 
294  /**
295  * \brief Sets the HOG confidence.
296  * \param[in] confidence
297  */
298  void
299  setPersonConfidence (float confidence);
300 
301  /**
302  * \brief Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
303  * \param[in] viewer PCL visualizer.
304  * \param[in] person_numbers progressive number representing the person.
305  */
306  void
307  drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number);
308 
309  /**
310  * \brief For sorting purpose: sort by distance.
311  */
312  friend bool operator< <>(const PersonCluster<PointT>& c1, const PersonCluster<PointT>& c2);
313 
314  protected:
315 
316  /**
317  * \brief PersonCluster initialization.
318  */
319  void init(
320  const PointCloudPtr& input_cloud,
321  const pcl::PointIndices& indices,
322  const Eigen::VectorXf& ground_coeffs,
323  float sqrt_ground_coeffs,
324  bool head_centroid,
325  bool vertical);
326 
327  };
328  } /* namespace people */
329 } /* namespace pcl */
330 #include <pcl/people/impl/person_cluster.hpp>
331 #endif /* PCL_PEOPLE_PERSON_CLUSTER_H_ */
Eigen::Vector3f & getCenter()
Returns the centroid.
Eigen::Vector3f & getTop()
Returns the top point.
boost::shared_ptr< PointCloud > PointCloudPtr
float person_confidence_
PersonCluster HOG confidence.
pcl::PointCloud< PointT > PointCloud
Eigen::Vector3f bottom_
Cluster bottom point.
float getAngleMax()
Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
Eigen::Vector3f tcenter_
Theoretical cluster center (between ttop_ and tbottom_).
float max_y_
Maximum y coordinate of the cluster points.
void drawTBoundingBox(pcl::visualization::PCLVisualizer &viewer, int person_number)
Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
float max_x_
Maximum x coordinate of the cluster points.
Eigen::Vector3f & getTBottom()
Returns the theoretical bottom point.
float c_z_
z coordinate of the cluster centroid.
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 angle_
Cluster centroid horizontal angle with respect to z axis.
float updateHeight(const Eigen::VectorXf &ground_coeffs)
Update the height of the cluster.
float min_z_
Minimum z coordinate of the cluster points.
float getPersonConfidence()
Returns the HOG confidence.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
float getAngleMin()
Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
float min_y_
Minimum y coordinate of the cluster points.
float sum_x_
Sum of x coordinates of the cluster points.
Eigen::Vector3f & getTCenter()
Returns the theoretical centroid (at half height).
float angle_min_
Minimum angle of the cluster points.
void setPersonConfidence(float confidence)
Sets the HOG confidence.
Eigen::Vector3f min_
Vector containing the minimum coordinates of the cluster.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
PersonCluster represents a class for representing information about a cluster containing a person...
float c_x_
x coordinate of the cluster centroid.
int n_
Number of cluster points.
Eigen::Vector3f center_
Cluster centroid.
void setHeight(float height)
Sets the cluster height.
float distance_
Cluster distance from the sensor.
Eigen::Vector3f ttop_
Theoretical cluster top.
float angle_max_
Maximum angle of the cluster points.
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
Eigen::Vector3f & getBottom()
Returns the bottom point.
float c_y_
y coordinate of the cluster centroid.
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.
Eigen::Vector3f max_
Vector containing the maximum coordinates of the cluster.
bool vertical_
If true, the sensor is considered to be vertically placed (portrait mode).
float sum_y_
Sum of y coordinates of the cluster points.
float sum_z_
Sum of z coordinates of the cluster points.
PCL Visualizer main class.
Eigen::Vector3f tbottom_
Theoretical cluster bottom (lying on the ground plane).
int getNumberPoints()
Returns the number of points of the cluster.
float max_z_
Maximum z coordinate of the cluster points.
float getAngle()
Returns the angle formed by the cluster's centroid with respect to the sensor (in radians)...
float getDistance()
Returns the distance of the cluster from the sensor.
float getHeight()
Returns the height of the cluster.
virtual ~PersonCluster()
Destructor.
Eigen::Vector3f & getTTop()
Returns the theoretical top point.
Eigen::Vector3f top_
Cluster top point.
float height_
Cluster height from the ground plane.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
float min_x_
Minimum x coordinate of the cluster points.
pcl::PointIndices points_indices_
Point cloud indices of the cluster points.