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_);