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.hpp 00037 * Created on: Nov 30, 2012 00038 * Author: Matteo Munaro 00039 */ 00040 00041 #ifndef PCL_PEOPLE_PERSON_CLUSTER_HPP_ 00042 #define PCL_PEOPLE_PERSON_CLUSTER_HPP_ 00043 00044 #include <pcl/people/person_cluster.h> 00045 00046 template <typename PointT> 00047 pcl::people::PersonCluster<PointT>::PersonCluster ( 00048 const PointCloudPtr& input_cloud, 00049 const pcl::PointIndices& indices, 00050 const Eigen::VectorXf& ground_coeffs, 00051 float sqrt_ground_coeffs, 00052 bool head_centroid, 00053 bool vertical) 00054 { 00055 init(input_cloud, indices, ground_coeffs, sqrt_ground_coeffs, head_centroid, vertical); 00056 } 00057 00058 template <typename PointT> void 00059 pcl::people::PersonCluster<PointT>::init ( 00060 const PointCloudPtr& input_cloud, 00061 const pcl::PointIndices& indices, 00062 const Eigen::VectorXf& ground_coeffs, 00063 float sqrt_ground_coeffs, 00064 bool head_centroid, 00065 bool vertical) 00066 { 00067 00068 vertical_ = vertical; 00069 head_centroid_ = head_centroid; 00070 person_confidence_ = std::numeric_limits<float>::quiet_NaN(); 00071 00072 min_x_ = 1000.0f; 00073 min_y_ = 1000.0f; 00074 min_z_ = 1000.0f; 00075 00076 max_x_ = -1000.0f; 00077 max_y_ = -1000.0f; 00078 max_z_ = -1000.0f; 00079 00080 sum_x_ = 0.0f; 00081 sum_y_ = 0.0f; 00082 sum_z_ = 0.0f; 00083 00084 n_ = 0; 00085 00086 points_indices_.indices = indices.indices; 00087 00088 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) 00089 { 00090 PointT* p = &input_cloud->points[*pit]; 00091 00092 min_x_ = std::min(p->x, min_x_); 00093 max_x_ = std::max(p->x, max_x_); 00094 sum_x_ += p->x; 00095 00096 min_y_ = std::min(p->y, min_y_); 00097 max_y_ = std::max(p->y, max_y_); 00098 sum_y_ += p->y; 00099 00100 min_z_ = std::min(p->z, min_z_); 00101 max_z_ = std::max(p->z, max_z_); 00102 sum_z_ += p->z; 00103 00104 n_++; 00105 } 00106 00107 c_x_ = sum_x_ / n_; 00108 c_y_ = sum_y_ / n_; 00109 c_z_ = sum_z_ / n_; 00110 00111 00112 Eigen::Vector4f height_point(c_x_, c_y_, c_z_, 1.0f); 00113 if(!vertical_) 00114 { 00115 height_point(1) = min_y_; 00116 distance_ = std::sqrt(c_x_ * c_x_ + c_z_ * c_z_); 00117 } 00118 else 00119 { 00120 height_point(0) = max_x_; 00121 distance_ = std::sqrt(c_y_ * c_y_ + c_z_ * c_z_); 00122 } 00123 00124 float height = std::fabs(height_point.dot(ground_coeffs)); 00125 height /= sqrt_ground_coeffs; 00126 height_ = height; 00127 00128 if(head_centroid_) 00129 { 00130 float sum_x = 0.0f; 00131 float sum_y = 0.0f; 00132 float sum_z = 0.0f; 00133 int n = 0; 00134 00135 float head_threshold_value; // vertical coordinate of the lowest head point 00136 if (!vertical_) 00137 { 00138 head_threshold_value = min_y_ + height_ / 8.0f; // head is suppose to be 1/8 of the human height 00139 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) 00140 { 00141 PointT* p = &input_cloud->points[*pit]; 00142 00143 if(p->y < head_threshold_value) 00144 { 00145 sum_x += p->x; 00146 sum_y += p->y; 00147 sum_z += p->z; 00148 n++; 00149 } 00150 } 00151 } 00152 else 00153 { 00154 head_threshold_value = max_x_ - height_ / 8.0f; // head is suppose to be 1/8 of the human height 00155 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) 00156 { 00157 PointT* p = &input_cloud->points[*pit]; 00158 00159 if(p->x > head_threshold_value) 00160 { 00161 sum_x += p->x; 00162 sum_y += p->y; 00163 sum_z += p->z; 00164 n++; 00165 } 00166 } 00167 } 00168 00169 c_x_ = sum_x / n; 00170 c_y_ = sum_y / n; 00171 c_z_ = sum_z / n; 00172 } 00173 00174 if(!vertical_) 00175 { 00176 float min_x = c_x_; 00177 float min_z = c_z_; 00178 float max_x = c_x_; 00179 float max_z = c_z_; 00180 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) 00181 { 00182 PointT* p = &input_cloud->points[*pit]; 00183 00184 min_x = std::min(p->x, min_x); 00185 max_x = std::max(p->x, max_x); 00186 min_z = std::min(p->z, min_z); 00187 max_z = std::max(p->z, max_z); 00188 } 00189 00190 angle_ = std::atan2(c_z_, c_x_); 00191 angle_max_ = std::max(std::atan2(min_z, min_x), std::atan2(max_z, min_x)); 00192 angle_min_ = std::min(std::atan2(min_z, max_x), std::atan2(max_z, max_x)); 00193 00194 Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f); 00195 float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2); 00196 float bottom_x = c_x_ - ground_coeffs(0) * t; 00197 float bottom_y = c_y_ - ground_coeffs(1) * t; 00198 float bottom_z = c_z_ - ground_coeffs(2) * t; 00199 00200 tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z); 00201 Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_; 00202 00203 ttop_ = v * height / v.norm() + tbottom_; 00204 tcenter_ = v * height * 0.5 / v.norm() + tbottom_; 00205 top_ = Eigen::Vector3f(c_x_, min_y_, c_z_); 00206 bottom_ = Eigen::Vector3f(c_x_, max_y_, c_z_); 00207 center_ = Eigen::Vector3f(c_x_, c_y_, c_z_); 00208 00209 min_ = Eigen::Vector3f(min_x_, min_y_, min_z_); 00210 00211 max_ = Eigen::Vector3f(max_x_, max_y_, max_z_); 00212 } 00213 else 00214 { 00215 float min_y = c_y_; 00216 float min_z = c_z_; 00217 float max_y = c_y_; 00218 float max_z = c_z_; 00219 for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) 00220 { 00221 PointT* p = &input_cloud->points[*pit]; 00222 00223 min_y = std::min(p->y, min_y); 00224 max_y = std::max(p->y, max_y); 00225 min_z = std::min(p->z, min_z); 00226 max_z = std::max(p->z, max_z); 00227 } 00228 00229 angle_ = std::atan2(c_z_, c_y_); 00230 angle_max_ = std::max(std::atan2(min_z_, min_y_), std::atan2(max_z_, min_y_)); 00231 angle_min_ = std::min(std::atan2(min_z_, max_y_), std::atan2(max_z_, max_y_)); 00232 00233 Eigen::Vector4f c_point(c_x_, c_y_, c_z_, 1.0f); 00234 float t = c_point.dot(ground_coeffs) / std::pow(sqrt_ground_coeffs, 2); 00235 float bottom_x = c_x_ - ground_coeffs(0) * t; 00236 float bottom_y = c_y_ - ground_coeffs(1) * t; 00237 float bottom_z = c_z_ - ground_coeffs(2) * t; 00238 00239 tbottom_ = Eigen::Vector3f(bottom_x, bottom_y, bottom_z); 00240 Eigen::Vector3f v = Eigen::Vector3f(c_x_, c_y_, c_z_) - tbottom_; 00241 00242 ttop_ = v * height / v.norm() + tbottom_; 00243 tcenter_ = v * height * 0.5 / v.norm() + tbottom_; 00244 top_ = Eigen::Vector3f(max_x_, c_y_, c_z_); 00245 bottom_ = Eigen::Vector3f(min_x_, c_y_, c_z_); 00246 center_ = Eigen::Vector3f(c_x_, c_y_, c_z_); 00247 00248 min_ = Eigen::Vector3f(min_x_, min_y_, min_z_); 00249 00250 max_ = Eigen::Vector3f(max_x_, max_y_, max_z_); 00251 } 00252 } 00253 00254 template <typename PointT> pcl::PointIndices& 00255 pcl::people::PersonCluster<PointT>::getIndices () 00256 { 00257 return (points_indices_); 00258 } 00259 00260 template <typename PointT> float 00261 pcl::people::PersonCluster<PointT>::getHeight () 00262 { 00263 return (height_); 00264 } 00265 00266 template <typename PointT> float 00267 pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs) 00268 { 00269 float sqrt_ground_coeffs = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); 00270 return (updateHeight(ground_coeffs, sqrt_ground_coeffs)); 00271 } 00272 00273 template <typename PointT> float 00274 pcl::people::PersonCluster<PointT>::updateHeight (const Eigen::VectorXf& ground_coeffs, float sqrt_ground_coeffs) 00275 { 00276 Eigen::Vector4f height_point; 00277 if (!vertical_) 00278 height_point << c_x_, min_y_, c_z_, 1.0f; 00279 else 00280 height_point << max_x_, c_y_, c_z_, 1.0f; 00281 00282 float height = std::fabs(height_point.dot(ground_coeffs)); 00283 height /= sqrt_ground_coeffs; 00284 height_ = height; 00285 return (height_); 00286 } 00287 00288 template <typename PointT> float 00289 pcl::people::PersonCluster<PointT>::getDistance () 00290 { 00291 return (distance_); 00292 } 00293 00294 template <typename PointT> Eigen::Vector3f& 00295 pcl::people::PersonCluster<PointT>::getTTop () 00296 { 00297 return (ttop_); 00298 } 00299 00300 template <typename PointT> Eigen::Vector3f& 00301 pcl::people::PersonCluster<PointT>::getTBottom () 00302 { 00303 return (tbottom_); 00304 } 00305 00306 template <typename PointT> Eigen::Vector3f& 00307 pcl::people::PersonCluster<PointT>::getTCenter () 00308 { 00309 return (tcenter_); 00310 } 00311 00312 template <typename PointT> Eigen::Vector3f& 00313 pcl::people::PersonCluster<PointT>::getTop () 00314 { 00315 return (top_); 00316 } 00317 00318 template <typename PointT> Eigen::Vector3f& 00319 pcl::people::PersonCluster<PointT>::getBottom () 00320 { 00321 return (bottom_); 00322 } 00323 00324 template <typename PointT> Eigen::Vector3f& 00325 pcl::people::PersonCluster<PointT>::getCenter () 00326 { 00327 return (center_); 00328 } 00329 00330 template <typename PointT> Eigen::Vector3f& 00331 pcl::people::PersonCluster<PointT>::getMin () 00332 { 00333 return (min_); 00334 } 00335 00336 template <typename PointT> Eigen::Vector3f& 00337 pcl::people::PersonCluster<PointT>::getMax () 00338 { 00339 return (max_); 00340 } 00341 00342 template <typename PointT> float 00343 pcl::people::PersonCluster<PointT>::getAngle () 00344 { 00345 return (angle_); 00346 } 00347 00348 template <typename PointT> 00349 float pcl::people::PersonCluster<PointT>::getAngleMax () 00350 { 00351 return (angle_max_); 00352 } 00353 00354 template <typename PointT> 00355 float pcl::people::PersonCluster<PointT>::getAngleMin () 00356 { 00357 return (angle_min_); 00358 } 00359 00360 template <typename PointT> 00361 int pcl::people::PersonCluster<PointT>::getNumberPoints () 00362 { 00363 return (n_); 00364 } 00365 00366 template <typename PointT> 00367 float pcl::people::PersonCluster<PointT>::getPersonConfidence () 00368 { 00369 return (person_confidence_); 00370 } 00371 00372 template <typename PointT> 00373 void pcl::people::PersonCluster<PointT>::setPersonConfidence (float confidence) 00374 { 00375 person_confidence_ = confidence; 00376 } 00377 00378 template <typename PointT> 00379 void pcl::people::PersonCluster<PointT>::setHeight (float height) 00380 { 00381 height_ = height; 00382 } 00383 00384 template <typename PointT> 00385 void pcl::people::PersonCluster<PointT>::drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number) 00386 { 00387 // draw theoretical person bounding box in the PCL viewer: 00388 pcl::ModelCoefficients coeffs; 00389 // translation 00390 coeffs.values.push_back (tcenter_[0]); 00391 coeffs.values.push_back (tcenter_[1]); 00392 coeffs.values.push_back (tcenter_[2]); 00393 // rotation 00394 coeffs.values.push_back (0.0); 00395 coeffs.values.push_back (0.0); 00396 coeffs.values.push_back (0.0); 00397 coeffs.values.push_back (1.0); 00398 // size 00399 if (vertical_) 00400 { 00401 coeffs.values.push_back (height_); 00402 coeffs.values.push_back (0.5); 00403 coeffs.values.push_back (0.5); 00404 } 00405 else 00406 { 00407 coeffs.values.push_back (0.5); 00408 coeffs.values.push_back (height_); 00409 coeffs.values.push_back (0.5); 00410 } 00411 00412 std::stringstream bbox_name; 00413 bbox_name << "bbox_person_" << person_number; 00414 viewer.removeShape (bbox_name.str()); 00415 viewer.addCube (coeffs, bbox_name.str()); 00416 viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name.str()); 00417 viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name.str()); 00418 00419 // std::stringstream confid; 00420 // confid << person_confidence_; 00421 // PointT position; 00422 // position.x = tcenter_[0]- 0.2; 00423 // position.y = ttop_[1]; 00424 // position.z = tcenter_[2]; 00425 // viewer.addText3D(confid.str().substr(0, 4), position, 0.1); 00426 } 00427 00428 template <typename PointT> 00429 pcl::people::PersonCluster<PointT>::~PersonCluster () 00430 { 00431 // Auto-generated destructor stub 00432 } 00433 #endif /* PCL_PEOPLE_PERSON_CLUSTER_HPP_ */