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>
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
int getNumberPoints()
Returns the number of points of the cluster.
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.
float getHeight()
Returns the height of the cluster.
std::vector< float > values
Eigen::Vector3f & getCenter()
Returns the centroid.
Eigen::Vector3f & getTCenter()
Returns the theoretical centroid (at half height).
std::vector< int > indices
float getAngle()
Returns the angle formed by the cluster's centroid with respect to the sensor (in radians)...
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()
Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
Eigen::Vector3f & getTTop()
Returns the theoretical top point.
Eigen::Vector3f & getBottom()
Returns the bottom point.
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.
Eigen::Vector3f & getTop()
Returns the top point.
boost::shared_ptr< PointCloud > PointCloudPtr
virtual ~PersonCluster()
Destructor.
float getAngleMax()
Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
float getPersonConfidence()
Returns the HOG confidence.
void setPersonConfidence(float confidence)
Sets the HOG confidence.
void drawTBoundingBox(pcl::visualization::PCLVisualizer &viewer, int person_number)
Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
float getDistance()
Returns the distance of the cluster from the sensor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
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.
float updateHeight(const Eigen::VectorXf &ground_coeffs)
Update the height of the cluster.
void setHeight(float height)
Sets the cluster height.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
Eigen::Vector3f & getTBottom()
Returns the theoretical bottom point.