41 #ifndef PCL_PEOPLE_PERSON_CLUSTER_HPP_
42 #define PCL_PEOPLE_PERSON_CLUSTER_HPP_
44 #include <pcl/people/person_cluster.h>
46 template <
typename Po
intT>
50 const Eigen::VectorXf& ground_coeffs,
51 float sqrt_ground_coeffs,
55 init(input_cloud, indices, ground_coeffs, sqrt_ground_coeffs, head_centroid, vertical);
58 template <
typename Po
intT>
void
62 const Eigen::VectorXf& ground_coeffs,
63 float sqrt_ground_coeffs,
69 head_centroid_ = head_centroid;
70 person_confidence_ = std::numeric_limits<float>::quiet_NaN();
86 points_indices_.indices = indices.
indices;
88 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
90 PointT* p = &input_cloud->points[*pit];
92 min_x_ = std::min(p->x, min_x_);
93 max_x_ = std::max(p->x, max_x_);
96 min_y_ = std::min(p->y, min_y_);
97 max_y_ = std::max(p->y, max_y_);
100 min_z_ = std::min(p->z, min_z_);
101 max_z_ = std::max(p->z, max_z_);
112 Eigen::Vector4f height_point(c_x_, c_y_, c_z_, 1.0f);
115 height_point(1) = min_y_;
116 distance_ = std::sqrt(c_x_ * c_x_ + c_z_ * c_z_);
120 height_point(0) = max_x_;
121 distance_ = std::sqrt(c_y_ * c_y_ + c_z_ * c_z_);
124 float height = std::fabs(height_point.dot(ground_coeffs));
125 height /= sqrt_ground_coeffs;
135 float head_threshold_value;
138 head_threshold_value = min_y_ + height_ / 8.0f;
139 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
141 PointT* p = &input_cloud->points[*pit];
143 if(p->y < head_threshold_value)
154 head_threshold_value = max_x_ - height_ / 8.0f;
155 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
157 PointT* p = &input_cloud->points[*pit];
159 if(p->x > head_threshold_value)
180 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
182 PointT* p = &input_cloud->points[*pit];
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);
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));
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;
200 tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
201 Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;
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_);
209 min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);
211 max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
219 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
221 PointT* p = &input_cloud->points[*pit];
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);
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_));
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;
239 tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z);
240 Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_;
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_);
248 min_ = Eigen::Vector3f(min_x_, min_y_, min_z_);
250 max_ = Eigen::Vector3f(max_x_, max_y_, max_z_);
257 return (points_indices_);
260 template <
typename Po
intT>
float
266 template <
typename Po
intT>
float
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));
273 template <
typename Po
intT>
float
276 Eigen::Vector4f height_point;
278 height_point << c_x_, min_y_, c_z_, 1.0f;
280 height_point << max_x_, c_y_, c_z_, 1.0f;
282 float height = std::fabs(height_point.dot(ground_coeffs));
283 height /= sqrt_ground_coeffs;
288 template <
typename Po
intT>
float
294 template <
typename Po
intT> Eigen::Vector3f&
300 template <
typename Po
intT> Eigen::Vector3f&
306 template <
typename Po
intT> Eigen::Vector3f&
312 template <
typename Po
intT> Eigen::Vector3f&
318 template <
typename Po
intT> Eigen::Vector3f&
324 template <
typename Po
intT> Eigen::Vector3f&
330 template <
typename Po
intT> Eigen::Vector3f&
336 template <
typename Po
intT> Eigen::Vector3f&
342 template <
typename Po
intT>
float
348 template <
typename Po
intT>
354 template <
typename Po
intT>
360 template <
typename Po
intT>
366 template <
typename Po
intT>
369 return (person_confidence_);
372 template <
typename Po
intT>
375 person_confidence_ = confidence;
378 template <
typename Po
intT>
384 template <
typename Po
intT>
390 coeffs.
values.push_back (tcenter_[0]);
391 coeffs.
values.push_back (tcenter_[1]);
392 coeffs.
values.push_back (tcenter_[2]);
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);
401 coeffs.
values.push_back (height_);
402 coeffs.
values.push_back (0.5);
403 coeffs.
values.push_back (0.5);
407 coeffs.
values.push_back (0.5);
408 coeffs.
values.push_back (height_);
409 coeffs.
values.push_back (0.5);
412 std::stringstream bbox_name;
413 bbox_name <<
"bbox_person_" << person_number;
415 viewer.
addCube (coeffs, bbox_name.str());
428 template <
typename Po
intT>
float getPersonConfidence() const
Returns the HOG confidence.
int getNumberPoints() const
Returns the number of points of the cluster.
void setPersonConfidence(float confidence)
Sets the HOG confidence.
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.
virtual ~PersonCluster()
Destructor.
std::vector< float > values
Eigen::Vector3f & getTTop()
Returns the theoretical top point.
Eigen::Vector3f & getCenter()
Returns the centroid.
float getDistance() const
Returns the distance of the cluster from the sensor.
typename PointCloud::Ptr PointCloudPtr
3 floats (R, G, B) going from 0.0 (dark) to 1.0 (light)
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
Eigen::Vector3f & getTCenter()
Returns the theoretical centroid (at half height).
float getAngle() const
Returns the angle formed by the cluster's centroid with respect to the sensor (in radians)...
PCL Visualizer main class.
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() const
Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
float getHeight() const
Returns the height of the cluster.
bool addCube(const pcl::ModelCoefficients &coefficients, const std::string &id="cube", int viewport=0)
Add a cube from a set of given model coefficients.
float updateHeight(const Eigen::VectorXf &ground_coeffs)
Update the height of the cluster.
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
float getAngleMax() const
Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
void drawTBoundingBox(pcl::visualization::PCLVisualizer &viewer, int person_number)
Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Vector3f & getTBottom()
Returns the theoretical bottom point.
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.
Eigen::Vector3f & getBottom()
Returns the bottom point.
void setHeight(float height)
Sets the cluster height.