Point Cloud Library (PCL)
1.7.0
|
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_ */