Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
46template <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
58template <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 (const auto& index : (points_indices_.indices))
89 {
90 PointT* p = &(*input_cloud)[index];
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 (const auto& index : (points_indices_.indices))
140 {
141 PointT* p = &(*input_cloud)[index];
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 (const auto& index : (points_indices_.indices))
156 {
157 PointT* p = &(*input_cloud)[index];
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 (const auto& index : (points_indices_.indices))
181 {
182 PointT* p = &(*input_cloud)[index];
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 (const auto& index : (points_indices_.indices))
220 {
221 PointT* p = &(*input_cloud)[index];
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
254template <typename PointT> pcl::PointIndices&
256{
257 return (points_indices_);
258}
259
260template <typename PointT> float
262{
263 return (height_);
264}
265
266template <typename PointT> float
267pcl::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
273template <typename PointT> float
274pcl::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
288template <typename PointT> float
290{
291 return (distance_);
292}
293
294template <typename PointT> Eigen::Vector3f&
296{
297 return (ttop_);
298}
299
300template <typename PointT> Eigen::Vector3f&
302{
303 return (tbottom_);
304}
305
306template <typename PointT> Eigen::Vector3f&
308{
309 return (tcenter_);
310}
311
312template <typename PointT> Eigen::Vector3f&
314{
315 return (top_);
316}
317
318template <typename PointT> Eigen::Vector3f&
320{
321 return (bottom_);
322}
323
324template <typename PointT> Eigen::Vector3f&
326{
327 return (center_);
328}
329
330template <typename PointT> Eigen::Vector3f&
332{
333 return (min_);
334}
335
336template <typename PointT> Eigen::Vector3f&
338{
339 return (max_);
340}
341
342template <typename PointT> float
344{
345 return (angle_);
346}
347
348template <typename PointT>
350{
351 return (angle_max_);
352}
353
354template <typename PointT>
356{
357 return (angle_min_);
358}
359
360template <typename PointT>
362{
363 return (n_);
364}
365
366template <typename PointT>
368{
369 return (person_confidence_);
370}
371
372template <typename PointT>
374{
375 person_confidence_ = confidence;
376}
377
378template <typename PointT>
380{
381 height_ = height;
382}
383
384template <typename PointT>
386{
387 // draw theoretical person bounding box in the PCL viewer:
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 const std::string bbox_name = "bbox_person_" + std::to_string(person_number);
413 viewer.removeShape (bbox_name);
414 viewer.addCube (coeffs, bbox_name);
417}
418
419template <typename PointT>
421{
422 // Auto-generated destructor stub
423}
424#endif /* PCL_PEOPLE_PERSON_CLUSTER_HPP_ */
float updateHeight(const Eigen::VectorXf &ground_coeffs)
Update the height of the cluster.
float getPersonConfidence() const
Returns the HOG confidence.
float getHeight() const
Returns the height of the cluster.
Eigen::Vector3f & getBottom()
Returns the bottom point.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
void setPersonConfidence(float confidence)
Sets the HOG confidence.
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.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
Eigen::Vector3f & getCenter()
Returns the centroid.
float getAngleMax() const
Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
void setHeight(float height)
Sets the cluster height.
typename PointCloud::Ptr PointCloudPtr
void drawTBoundingBox(pcl::visualization::PCLVisualizer &viewer, int person_number)
Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
Eigen::Vector3f & getTBottom()
Returns the theoretical bottom point.
Eigen::Vector3f & getTTop()
Returns the theoretical top point.
float getAngleMin() const
Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
virtual ~PersonCluster()
Destructor.
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 & getTop()
Returns the top point.
float getDistance() const
Returns the distance of the cluster from the sensor.
float getAngle() const
Returns the angle formed by the cluster's centroid with respect to the sensor (in radians).
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
Eigen::Vector3f & getTCenter()
Returns the theoretical centroid (at half height).
int getNumberPoints() const
Returns the number of points of the cluster.
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.
bool setShapeRenderingProperties(int property, double value, const std::string &id, int viewport=0)
Set the rendering properties of a shape.
bool removeShape(const std::string &id="cloud", int viewport=0)
Removes an added shape from screen (line, polygon, etc.), based on a given ID.
@ PCL_VISUALIZER_COLOR
3 floats (R, G, B) going from 0.0 (dark) to 1.0 (light)
Definition common.h:107
@ PCL_VISUALIZER_LINE_WIDTH
Integer starting from 1.
Definition common.h:105
std::vector< float > values
A point structure representing Euclidean xyz coordinates, and the RGB color.