door_handle_detector: statistics.h Source File

00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id$
00028  *
00029  */
00030 
00033 #ifndef _CLOUD_GEOMETRY_STATISTICS_H_
00034 #define _CLOUD_GEOMETRY_STATISTICS_H_
00035 
00036 // ROS includes
00037 #include <sensor_msgs/PointCloud.h>
00038 #include <geometry_msgs/Point32.h>
00039 #include <geometry_msgs/Polygon.h>
00040 #include <cfloat>
00041 
00042 namespace cloud_geometry
00043 {
00044 
00045   namespace statistics
00046   {
00047 
00049 
00054     inline void
00055       getLargestDiagonalIndices (const geometry_msgs::Polygon &poly, int &min_idx, int &max_idx)
00056     {
00057       double largest_diagonal = -FLT_MAX;
00058       for (unsigned int i = 0; i < poly.points.size (); i++)
00059       {
00060         for (unsigned int j = i; j < poly.points.size (); j++)
00061         {
00062           double current_diagonal =
00063                                     (poly.points[i].x - poly.points[j].x) * (poly.points[i].x - poly.points[j].x) +
00064                                     (poly.points[i].y - poly.points[j].y) * (poly.points[i].y - poly.points[j].y) +
00065                                     (poly.points[i].z - poly.points[j].z) * (poly.points[i].z - poly.points[j].z);
00066           if (current_diagonal > largest_diagonal)
00067           {
00068             min_idx = i;
00069             max_idx = j;
00070             largest_diagonal = current_diagonal;
00071           }
00072         }
00073       }
00074     }
00075 
00077 
00082     inline void
00083       getLargestDiagonalIndices (const sensor_msgs::PointCloud &points, int &min_idx, int &max_idx)
00084     {
00085       double largest_diagonal = -FLT_MAX;
00086       for (unsigned int i = 0; i < points.points.size (); i++)
00087       {
00088         for (unsigned int j = i; j < points.points.size (); j++)
00089         {
00090           double current_diagonal =
00091                                     (points.points.at (i).x - points.points.at (j).x) * (points.points.at (i).x - points.points.at (j).x) +
00092                                     (points.points.at (i).y - points.points.at (j).y) * (points.points.at (i).y - points.points.at (j).y) +
00093                                     (points.points.at (i).z - points.points.at (j).z) * (points.points.at (i).z - points.points.at (j).z);
00094           if (current_diagonal > largest_diagonal)
00095           {
00096             min_idx = i;
00097             max_idx = j;
00098             largest_diagonal = current_diagonal;
00099           }
00100         }
00101       }
00102     }
00103 
00105 
00111     inline void
00112       getLargestDiagonalIndices (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, int &min_idx, int &max_idx)
00113     {
00114       double largest_diagonal = -FLT_MAX;
00115       for (unsigned int i = 0; i < indices.size (); i++)
00116       {
00117         for (unsigned int j = i; j < indices.size (); j++)
00118         {
00119           double current_diagonal =
00120                                     (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) * (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) +
00121                                     (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y) * (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y) +
00122                                     (points.points.at (indices.at (i)).z - points.points.at (indices.at (j)).z) * (points.points.at (indices.at (i)).z - points.points.at (indices.at (j)).z);
00123           if (current_diagonal > largest_diagonal)
00124           {
00125             min_idx = i;
00126             max_idx = j;
00127             largest_diagonal = current_diagonal;
00128           }
00129         }
00130       }
00131     }
00132 
00134 
00139     inline void
00140       getLargestDiagonalPoints (const geometry_msgs::Polygon &poly, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00141     {
00142       double largest_diagonal = -FLT_MAX;
00143       for (unsigned int i = 0; i < poly.points.size (); i++)
00144       {
00145         for (unsigned int j = i; j < poly.points.size (); j++)
00146         {
00147           double current_diagonal =
00148                                     (poly.points[i].x - poly.points[j].x) * (poly.points[i].x - poly.points[j].x) +
00149                                     (poly.points[i].y - poly.points[j].y) * (poly.points[i].y - poly.points[j].y) +
00150                                     (poly.points[i].z - poly.points[j].z) * (poly.points[i].z - poly.points[j].z);
00151           if (current_diagonal > largest_diagonal)
00152           {
00153             min_p.x = poly.points[i].x; min_p.y = poly.points[i].y; min_p.z = poly.points[i].z;
00154             max_p.x = poly.points[j].x; max_p.y = poly.points[j].y; max_p.z = poly.points[j].z;
00155             largest_diagonal = current_diagonal;
00156           }
00157         }
00158       }
00159     }
00160 
00162 
00167     inline void
00168       getLargestDiagonalPoints (const sensor_msgs::PointCloud &points, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00169     {
00170       double largest_diagonal = -FLT_MAX;
00171       for (unsigned int i = 0; i < points.points.size (); i++)
00172       {
00173         for (unsigned int j = i; j < points.points.size (); j++)
00174         {
00175           double current_diagonal =
00176                                     (points.points.at (i).x - points.points.at (j).x) * (points.points.at (i).x - points.points.at (j).x) +
00177                                     (points.points.at (i).y - points.points.at (j).y) * (points.points.at (i).y - points.points.at (j).y) +
00178                                     (points.points.at (i).z - points.points.at (j).z) * (points.points.at (i).z - points.points.at (j).z);
00179           if (current_diagonal > largest_diagonal)
00180           {
00181             min_p.x = points.points.at (i).x; min_p.y = points.points.at (i).y; min_p.z = points.points.at (i).z;
00182             max_p.x = points.points.at (j).x; max_p.y = points.points.at (j).y; max_p.z = points.points.at (j).z;
00183             largest_diagonal = current_diagonal;
00184           }
00185         }
00186       }
00187     }
00188 
00190 
00196     inline void
00197       getLargestDiagonalPoints (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00198     {
00199       double largest_diagonal = -FLT_MAX;
00200       for (unsigned int i = 0; i < indices.size (); i++)
00201       {
00202         for (unsigned int j = i; j < indices.size (); j++)
00203         {
00204           double current_diagonal =
00205                                     (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) * (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) +
00206                                     (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y) * (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y) +
00207                                     (points.points.at (indices.at (i)).z - points.points.at (indices.at (j)).z) * (points.points.at (indices.at (i)).z - points.points.at (indices.at (j)).z);
00208           if (current_diagonal > largest_diagonal)
00209           {
00210             min_p.x = points.points.at (indices.at (i)).x; min_p.y = points.points.at (indices.at (i)).y; min_p.z = points.points.at (indices.at (i)).z;
00211             max_p.x = points.points.at (indices.at (j)).x; max_p.y = points.points.at (indices.at (j)).y; max_p.z = points.points.at (indices.at (j)).z;
00212             largest_diagonal = current_diagonal;
00213           }
00214         }
00215       }
00216     }
00217 
00218     geometry_msgs::Point32 computeMedian (const sensor_msgs::PointCloud &points);
00219     geometry_msgs::Point32 computeMedian (const sensor_msgs::PointCloud &points, const std::vector<int> &indices);
00220 
00221     double computeMedianAbsoluteDeviation (const sensor_msgs::PointCloud &points, double sigma);
00222     double computeMedianAbsoluteDeviation (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double sigma);
00223 
00224 
00225 
00227 
00232     inline void
00233       getLargestXYPoints (const geometry_msgs::Polygon &poly, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00234     {
00235       double largest_xy = -FLT_MAX;
00236       for (unsigned int i = 0; i < poly.points.size (); i++)
00237       {
00238         for (unsigned int j = i; j < poly.points.size (); j++)
00239         {
00240           double current_xy = (poly.points[i].x - poly.points[j].x) * (poly.points[i].x - poly.points[j].x) +
00241                               (poly.points[i].y - poly.points[j].y) * (poly.points[i].y - poly.points[j].y);
00242           if (current_xy > largest_xy)
00243           {
00244             min_p.x = poly.points[i].x; min_p.y = poly.points[i].y; min_p.z = 0;
00245             max_p.x = poly.points[j].x; max_p.y = poly.points[j].y; max_p.z = 0;
00246             largest_xy = current_xy;
00247           }
00248         }
00249       }
00250     }
00251 
00252 
00253 
00255 
00261     inline void
00262       getLargestXYPoints (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00263     {
00264       double largest_xy = -FLT_MAX;
00265       for (unsigned int i = 0; i < indices.size (); i++)
00266       {
00267         for (unsigned int j = i; j < indices.size (); j++)
00268         {
00269           double current_xy = (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) *
00270                               (points.points.at (indices.at (i)).x - points.points.at (indices.at (j)).x) +
00271                               (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y) *
00272                               (points.points.at (indices.at (i)).y - points.points.at (indices.at (j)).y);
00273           if (current_xy > largest_xy )
00274           {
00275             min_p.x = points.points.at (indices.at (i)).x;
00276             min_p.y = points.points.at (indices.at (i)).y;
00277             min_p.z = 0;
00278             max_p.x = points.points.at (indices.at (j)).x;
00279             max_p.y = points.points.at (indices.at (j)).y;
00280             max_p.z = 0;
00281             largest_xy = current_xy;
00282           }
00283         }
00284       }
00285     }
00286 
00287 
00288 
00290 
00296     inline double
00297       computeCentralizedMoment (const sensor_msgs::PointCloud &points, double p, double q, double r)
00298     {
00299       double result = 0.0;
00300       for (unsigned int cp = 0; cp < points.points.size (); cp++)
00301         result += pow (points.points[cp].x, p) * pow (points.points[cp].y, q) * pow (points.points[cp].z, r);
00302 
00303       return (result);
00304     }
00305 
00307 
00314     inline double
00315       computeCentralizedMoment (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double p, double q, double r)
00316     {
00317       double result = 0.0;
00318       for (unsigned int cp = 0; cp < indices.size (); cp++)
00319         result += pow (points.points.at (indices.at (cp)).x, p) * pow (points.points.at (indices.at (cp)).y, q) * pow (points.points.at (indices.at (cp)).z, r);
00320 
00321       return (result);
00322     }
00323 
00325 
00330     inline void
00331       getMinMax (const sensor_msgs::PointCloud &points, geometry_msgs::Point32 &minP, geometry_msgs::Point32 &maxP)
00332     {
00333       minP.x = minP.y = minP.z = FLT_MAX;
00334       maxP.x = maxP.y = maxP.z = -FLT_MAX;
00335 
00336       for (unsigned int i = 0; i < points.points.size (); i++)
00337       {
00338         minP.x = (points.points[i].x < minP.x) ? points.points[i].x : minP.x;
00339         minP.y = (points.points[i].y < minP.y) ? points.points[i].y : minP.y;
00340         minP.z = (points.points[i].z < minP.z) ? points.points[i].z : minP.z;
00341 
00342         maxP.x = (points.points[i].x > maxP.x) ? points.points[i].x : maxP.x;
00343         maxP.y = (points.points[i].y > maxP.y) ? points.points[i].y : maxP.y;
00344         maxP.z = (points.points[i].z > maxP.z) ? points.points[i].z : maxP.z;
00345       }
00346     }
00347 
00348 
00350 
00355     inline void
00356       getMinMax (const geometry_msgs::Polygon &poly, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00357     {
00358       min_p.x = min_p.y = min_p.z = FLT_MAX;
00359       max_p.x = max_p.y = max_p.z = -FLT_MAX;
00360 
00361       for (unsigned int i = 0; i < poly.points.size (); i++)
00362       {
00363         min_p.x = (poly.points[i].x < min_p.x) ? poly.points[i].x : min_p.x;
00364         min_p.y = (poly.points[i].y < min_p.y) ? poly.points[i].y : min_p.y;
00365         min_p.z = (poly.points[i].z < min_p.z) ? poly.points[i].z : min_p.z;
00366 
00367         max_p.x = (poly.points[i].x > max_p.x) ? poly.points[i].x : max_p.x;
00368         max_p.y = (poly.points[i].y > max_p.y) ? poly.points[i].y : max_p.y;
00369         max_p.z = (poly.points[i].z > max_p.z) ? poly.points[i].z : max_p.z;
00370       }
00371     }
00372 
00374 
00380     inline void
00381       getMinMax (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 &min_p, geometry_msgs::Point32 &max_p)
00382     {
00383       min_p.x = min_p.y = min_p.z = FLT_MAX;
00384       max_p.x = max_p.y = max_p.z = -FLT_MAX;
00385 
00386       for (unsigned int i = 0; i < indices.size (); i++)
00387       {
00388         min_p.x = (points.points.at (indices.at (i)).x < min_p.x) ? points.points.at (indices.at (i)).x : min_p.x;
00389         min_p.y = (points.points.at (indices.at (i)).y < min_p.y) ? points.points.at (indices.at (i)).y : min_p.y;
00390         min_p.z = (points.points.at (indices.at (i)).z < min_p.z) ? points.points.at (indices.at (i)).z : min_p.z;
00391 
00392         max_p.x = (points.points.at (indices.at (i)).x > max_p.x) ? points.points.at (indices.at (i)).x : max_p.x;
00393         max_p.y = (points.points.at (indices.at (i)).y > max_p.y) ? points.points.at (indices.at (i)).y : max_p.y;
00394         max_p.z = (points.points.at (indices.at (i)).z > max_p.z) ? points.points.at (indices.at (i)).z : max_p.z;
00395       }
00396     }
00397 
00399 
00407     inline void
00408       getMinMax (const sensor_msgs::PointCloud &points, geometry_msgs::Point32 &min_pt, geometry_msgs::Point32 &max_pt,
00409                  int c_idx, double cut_distance)
00410     {
00411       min_pt.x = min_pt.y = min_pt.z = FLT_MAX;
00412       max_pt.x = max_pt.y = max_pt.z = -FLT_MAX;
00413 
00414       for (unsigned int i = 0; i < points.points.size (); i++)
00415       {
00416         if (c_idx != -1 && points.channels[c_idx].values[i] > cut_distance)
00417           continue;
00418         min_pt.x = (points.points[i].x < min_pt.x) ? points.points[i].x : min_pt.x;
00419         min_pt.y = (points.points[i].y < min_pt.y) ? points.points[i].y : min_pt.y;
00420         min_pt.z = (points.points[i].z < min_pt.z) ? points.points[i].z : min_pt.z;
00421 
00422         max_pt.x = (points.points[i].x > max_pt.x) ? points.points[i].x : max_pt.x;
00423         max_pt.y = (points.points[i].y > max_pt.y) ? points.points[i].y : max_pt.y;
00424         max_pt.z = (points.points[i].z > max_pt.z) ? points.points[i].z : max_pt.z;
00425       }
00426     }
00427 
00429 
00437     inline void
00438       getMinMax (sensor_msgs::PointCloudConstPtr points, geometry_msgs::Point32 &min_pt, geometry_msgs::Point32 &max_pt,
00439                  int c_idx, double cut_distance)
00440     {
00441       min_pt.x = min_pt.y = min_pt.z = FLT_MAX;
00442       max_pt.x = max_pt.y = max_pt.z = -FLT_MAX;
00443 
00444       for (unsigned int i = 0; i < points->points.size (); i++)
00445       {
00446         if (c_idx != -1 && points->channels[c_idx].values[i] > cut_distance)
00447           continue;
00448         min_pt.x = (points->points[i].x < min_pt.x) ? points->points[i].x : min_pt.x;
00449         min_pt.y = (points->points[i].y < min_pt.y) ? points->points[i].y : min_pt.y;
00450         min_pt.z = (points->points[i].z < min_pt.z) ? points->points[i].z : min_pt.z;
00451 
00452         max_pt.x = (points->points[i].x > max_pt.x) ? points->points[i].x : max_pt.x;
00453         max_pt.y = (points->points[i].y > max_pt.y) ? points->points[i].y : max_pt.y;
00454         max_pt.z = (points->points[i].z > max_pt.z) ? points->points[i].z : max_pt.z;
00455       }
00456     }
00457 
00459 
00468     inline void
00469       getMinMax (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 &min_pt, geometry_msgs::Point32 &max_pt,
00470                  int c_idx, double cut_distance)
00471     {
00472       min_pt.x = min_pt.y = min_pt.z = FLT_MAX;
00473       max_pt.x = max_pt.y = max_pt.z = -FLT_MAX;
00474 
00475       for (unsigned int i = 0; i < indices.size (); i++)
00476       {
00477         if (c_idx != -1 && points.channels[c_idx].values[indices.at (i)] > cut_distance)
00478           continue;
00479         min_pt.x = (points.points[indices.at (i)].x < min_pt.x) ? points.points[indices.at (i)].x : min_pt.x;
00480         min_pt.y = (points.points[indices.at (i)].y < min_pt.y) ? points.points[indices.at (i)].y : min_pt.y;
00481         min_pt.z = (points.points[indices.at (i)].z < min_pt.z) ? points.points[indices.at (i)].z : min_pt.z;
00482 
00483         max_pt.x = (points.points[indices.at (i)].x > max_pt.x) ? points.points[indices.at (i)].x : max_pt.x;
00484         max_pt.y = (points.points[indices.at (i)].y > max_pt.y) ? points.points[indices.at (i)].y : max_pt.y;
00485         max_pt.z = (points.points[indices.at (i)].z > max_pt.z) ? points.points[indices.at (i)].z : max_pt.z;
00486       }
00487     }
00488 
00490 
00499     inline void
00500       getMinMax (sensor_msgs::PointCloudConstPtr points, const std::vector<int> &indices, geometry_msgs::Point32 &min_pt, geometry_msgs::Point32 &max_pt,
00501                  int c_idx, double cut_distance)
00502     {
00503       min_pt.x = min_pt.y = min_pt.z = FLT_MAX;
00504       max_pt.x = max_pt.y = max_pt.z = -FLT_MAX;
00505 
00506       for (unsigned int i = 0; i < indices.size (); i++)
00507       {
00508         if (c_idx != -1 && points->channels[c_idx].values[indices.at (i)] > cut_distance)
00509           continue;
00510         min_pt.x = (points->points[indices.at (i)].x < min_pt.x) ? points->points[indices.at (i)].x : min_pt.x;
00511         min_pt.y = (points->points[indices.at (i)].y < min_pt.y) ? points->points[indices.at (i)].y : min_pt.y;
00512         min_pt.z = (points->points[indices.at (i)].z < min_pt.z) ? points->points[indices.at (i)].z : min_pt.z;
00513 
00514         max_pt.x = (points->points[indices.at (i)].x > max_pt.x) ? points->points[indices.at (i)].x : max_pt.x;
00515         max_pt.y = (points->points[indices.at (i)].y > max_pt.y) ? points->points[indices.at (i)].y : max_pt.y;
00516         max_pt.z = (points->points[indices.at (i)].z > max_pt.z) ? points->points[indices.at (i)].z : max_pt.z;
00517       }
00518     }
00519 
00520     void getChannelMeanStd (const sensor_msgs::PointCloud &points, int d_idx, double &mean, double &stddev);
00521     void getChannelMeanStd (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx, double &mean, double &stddev);
00522 
00523     void selectPointsOutsideDistribution (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx,
00524                                           double mean, double stddev, double alpha, std::vector<int> &inliers);
00525     void selectPointsInsideDistribution (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx,
00526                                          double mean, double stddev, double alpha, std::vector<int> &inliers);
00527 
00529 
00534     inline void
00535       getMeanStd (const std::vector<int> &values, double &mean, double &stddev)
00536     {
00537       double sum = 0, sq_sum = 0;
00538 
00539       for (unsigned int i = 0; i < values.size (); i++)
00540       {
00541         sum += values.at (i);
00542         sq_sum += values.at (i) * values.at (i);
00543       }
00544       mean = sum / values.size ();
00545       double variance = (double)(sq_sum - sum * sum / values.size ()) / (values.size () - 1);
00546       stddev = sqrt (variance);
00547     }
00548 
00549     void getTrimean (std::vector<int> values, double &trimean);
00550 
00551   }
00552 }
00553 
00554 #endif