Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/people/include/pcl/people/person_cluster.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2013-, Open Perception, Inc.
00006  *
00007  * All rights reserved.
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * * Redistributions of source code must retain the above copyright
00014  * notice, this list of conditions and the following disclaimer.
00015  * * Redistributions in binary form must reproduce the above
00016  * copyright notice, this list of conditions and the following
00017  * disclaimer in the documentation and/or other materials provided
00018  * with the distribution.
00019  * * Neither the name of the copyright holder(s) nor the names of its
00020  * contributors may be used to endorse or promote products derived
00021  * from this software without specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  * POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * person_cluster.h
00037  * Created on: Nov 30, 2012
00038  * Author: Matteo Munaro
00039  */
00040 
00041 #ifndef PCL_PEOPLE_PERSON_CLUSTER_H_
00042 #define PCL_PEOPLE_PERSON_CLUSTER_H_
00043 
00044 #include <pcl/point_types.h>
00045 #include <pcl/visualization/pcl_visualizer.h>
00046 
00047 namespace pcl
00048 {
00049   namespace people
00050   {
00051     /** \brief @b PersonCluster represents a class for representing information about a cluster containing a person.
00052      * \author Filippo Basso, Matteo Munaro
00053      * \ingroup people
00054      */
00055     template <typename PointT> class PersonCluster;
00056     template <typename PointT> bool operator<(const PersonCluster<PointT>& c1, const PersonCluster<PointT>& c2);
00057 
00058     template <typename PointT>
00059     class PersonCluster
00060     {
00061     protected:
00062 
00063       bool head_centroid_;
00064 
00065       /** \brief Minimum x coordinate of the cluster points. */
00066       float min_x_;
00067       /** \brief Minimum y coordinate of the cluster points. */
00068       float min_y_;
00069       /** \brief Minimum z coordinate of the cluster points. */
00070       float min_z_;
00071 
00072       /** \brief Maximum x coordinate of the cluster points. */
00073       float max_x_;
00074       /** \brief Maximum y coordinate of the cluster points. */
00075       float max_y_;
00076       /** \brief Maximum z coordinate of the cluster points. */
00077       float max_z_;
00078 
00079       /** \brief Sum of x coordinates of the cluster points. */
00080       float sum_x_;
00081       /** \brief Sum of y coordinates of the cluster points. */
00082       float sum_y_;
00083       /** \brief Sum of z coordinates of the cluster points. */
00084       float sum_z_;
00085 
00086       /** \brief Number of cluster points. */
00087       int n_;
00088 
00089       /** \brief x coordinate of the cluster centroid. */
00090       float c_x_;
00091       /** \brief y coordinate of the cluster centroid. */
00092       float c_y_;
00093       /** \brief z coordinate of the cluster centroid. */
00094       float c_z_;
00095 
00096       /** \brief Cluster height from the ground plane. */
00097       float height_;
00098 
00099       /** \brief Cluster distance from the sensor. */
00100       float distance_;
00101       /** \brief Cluster centroid horizontal angle with respect to z axis. */
00102       float angle_;
00103 
00104       /** \brief Maximum angle of the cluster points. */
00105       float angle_max_;
00106       /** \brief Minimum angle of the cluster points. */
00107       float angle_min_;
00108       
00109       /** \brief Cluster top point. */
00110       Eigen::Vector3f top_;
00111       /** \brief Cluster bottom point. */
00112       Eigen::Vector3f bottom_;
00113       /** \brief Cluster centroid. */
00114       Eigen::Vector3f center_;
00115       
00116       /** \brief Theoretical cluster top. */
00117       Eigen::Vector3f ttop_;
00118       /** \brief Theoretical cluster bottom (lying on the ground plane). */
00119       Eigen::Vector3f tbottom_;
00120       /** \brief Theoretical cluster center (between ttop_ and tbottom_). */
00121       Eigen::Vector3f tcenter_;
00122 
00123       /** \brief Vector containing the minimum coordinates of the cluster. */
00124       Eigen::Vector3f min_;
00125       /** \brief Vector containing the maximum coordinates of the cluster. */
00126       Eigen::Vector3f max_;
00127 
00128       /** \brief Point cloud indices of the cluster points. */
00129       pcl::PointIndices points_indices_;
00130 
00131       /** \brief If true, the sensor is considered to be vertically placed (portrait mode). */
00132       bool vertical_;
00133       /** \brief PersonCluster HOG confidence. */
00134       float person_confidence_;
00135 
00136     public:
00137 
00138       typedef pcl::PointCloud<PointT> PointCloud;
00139       typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00140       typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00141 
00142       /** \brief Constructor. */
00143       PersonCluster (
00144           const PointCloudPtr& input_cloud,
00145           const pcl::PointIndices& indices,
00146           const Eigen::VectorXf& ground_coeffs,
00147           float sqrt_ground_coeffs,
00148           bool head_centroid,
00149           bool vertical = false);
00150 
00151       /** \brief Destructor. */
00152       virtual ~PersonCluster ();
00153 
00154       /**
00155        * \brief Returns the height of the cluster.
00156        * \return the height of the cluster.
00157        */
00158       float
00159       getHeight ();
00160 
00161       /**
00162        * \brief Update the height of the cluster.
00163        * \param[in] ground_coeffs The coefficients of the ground plane.
00164        * \return the height of the cluster.
00165        */
00166       float
00167       updateHeight (const Eigen::VectorXf& ground_coeffs);
00168 
00169       /**
00170        * \brief Update the height of the cluster.
00171        * \param[in] ground_coeffs The coefficients of the ground plane.
00172        * \param[in] sqrt_ground_coeffs The norm of the vector [a, b, c] where a, b and c are the first
00173        * three coefficients of the ground plane (ax + by + cz + d = 0).
00174        * \return the height of the cluster.
00175        */
00176       float
00177       updateHeight (const Eigen::VectorXf& ground_coeffs, float sqrt_ground_coeffs);
00178 
00179       /**
00180        * \brief Returns the distance of the cluster from the sensor.
00181        * \return the distance of the cluster (its centroid) from the sensor without considering the
00182        * y dimension.
00183        */
00184       float
00185       getDistance ();
00186 
00187       /**
00188        * \brief Returns the angle formed by the cluster's centroid with respect to the sensor (in radians).
00189        * \return the angle formed by the cluster's centroid with respect to the sensor (in radians).
00190        */
00191       float
00192       getAngle ();
00193 
00194       /**
00195        * \brief Returns the minimum angle formed by the cluster with respect to the sensor (in radians).
00196        * \return the minimum angle formed by the cluster with respect to the sensor (in radians).
00197        */
00198       float
00199       getAngleMin ();
00200 
00201       /**
00202        * \brief Returns the maximum angle formed by the cluster with respect to the sensor (in radians).
00203        * \return the maximum angle formed by the cluster with respect to the sensor (in radians).
00204        */
00205       float
00206       getAngleMax ();
00207 
00208       /**
00209        * \brief Returns the indices of the point cloud points corresponding to the cluster.
00210        * \return the indices of the point cloud points corresponding to the cluster.
00211        */
00212       pcl::PointIndices&
00213       getIndices ();
00214 
00215       /**
00216        * \brief Returns the theoretical top point.
00217        * \return the theoretical top point.
00218        */
00219       Eigen::Vector3f&
00220       getTTop ();
00221 
00222       /**
00223        * \brief Returns the theoretical bottom point.
00224        * \return the theoretical bottom point.
00225        */
00226       Eigen::Vector3f&
00227       getTBottom ();
00228 
00229       /**
00230        * \brief Returns the theoretical centroid (at half height).
00231        * \return the theoretical centroid (at half height).
00232        */
00233       Eigen::Vector3f&
00234       getTCenter ();
00235 
00236       /**
00237        * \brief Returns the top point.
00238        * \return the top point.
00239        */
00240       Eigen::Vector3f&
00241       getTop ();
00242 
00243       /**
00244        * \brief Returns the bottom point.
00245        * \return the bottom point.
00246        */
00247       Eigen::Vector3f&
00248       getBottom ();
00249 
00250       /**
00251        * \brief Returns the centroid.
00252        * \return the centroid.
00253        */
00254       Eigen::Vector3f&
00255       getCenter ();  
00256 
00257       //Eigen::Vector3f& getTMax();
00258 
00259       /**
00260        * \brief Returns the point formed by min x, min y and min z.
00261        * \return the point formed by min x, min y and min z.
00262        */
00263       Eigen::Vector3f&
00264       getMin ();
00265 
00266       /**
00267        * \brief Returns the point formed by max x, max y and max z.
00268        * \return the point formed by max x, max y and max z.
00269        */
00270       Eigen::Vector3f&
00271       getMax ();
00272 
00273       /**
00274        * \brief Returns the HOG confidence.
00275        * \return the HOG confidence.
00276        */
00277       float
00278       getPersonConfidence ();
00279 
00280       /**
00281        * \brief Returns the number of points of the cluster.
00282        * \return the number of points of the cluster.
00283        */
00284       int
00285       getNumberPoints ();
00286 
00287       /**
00288        * \brief Sets the cluster height.
00289        * \param[in] height
00290        */
00291       void
00292       setHeight (float height);
00293 
00294       /**
00295        * \brief Sets the HOG confidence.
00296        * \param[in] confidence
00297        */
00298       void
00299       setPersonConfidence (float confidence);
00300 
00301       /**
00302        * \brief Draws the theoretical 3D bounding box of the cluster in the PCL visualizer.
00303        * \param[in] viewer PCL visualizer.
00304        * \param[in] person_numbers progressive number representing the person.
00305        */
00306       void
00307       drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number);
00308 
00309       /**
00310        * \brief For sorting purpose: sort by distance.
00311        */
00312       friend bool operator< <>(const PersonCluster<PointT>& c1, const PersonCluster<PointT>& c2);
00313 
00314     protected:
00315 
00316       /**
00317        * \brief PersonCluster initialization.
00318        */
00319       void init(
00320           const PointCloudPtr& input_cloud,
00321           const pcl::PointIndices& indices,
00322           const Eigen::VectorXf& ground_coeffs,
00323           float sqrt_ground_coeffs,
00324           bool head_centroid,
00325           bool vertical);
00326 
00327     };
00328   } /* namespace people */
00329 } /* namespace pcl */
00330 #include <pcl/people/impl/person_cluster.hpp>
00331 #endif /* PCL_PEOPLE_PERSON_CLUSTER_H_ */