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 (
const auto& index : (points_indices_.indices))
90 PointT* p = &(*input_cloud)[index];
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 (
const auto& index : (points_indices_.indices))
141 PointT* p = &(*input_cloud)[index];
143 if(p->y < head_threshold_value)
154 head_threshold_value = max_x_ - height_ / 8.0f;
155 for (
const auto& index : (points_indices_.indices))
157 PointT* p = &(*input_cloud)[index];
159 if(p->x > head_threshold_value)
180 for (
const auto& index : (points_indices_.indices))
182 PointT* p = &(*input_cloud)[index];
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 (
const auto& index : (points_indices_.indices))
221 PointT* p = &(*input_cloud)[index];
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>