pcl: common.h Source File

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: common.h 35969 2011-02-11 22:56:54Z aaldoma $
00035  *
00036  */
00037 
00038 #ifndef PCL_COMMON_H_
00039 #define PCL_COMMON_H_
00040 
00041 #include <pcl/pcl_base.h>
00042 #include <pcl/io/io.h>
00043 #include <cfloat>
00044 
00045 /*
00046  * Define standard C methods and C++ classes that are common to all methods
00047  */
00048 
00049 namespace pcl
00050 {
00056   inline double getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2);
00057 
00063   inline void getMeanStd (const std::vector<float> &values, double &mean, double &stddev);
00064 
00071   template <typename PointT> 
00072   inline void getPointsInBox (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt,
00073                               Eigen::Vector4f &max_pt, std::vector<int> &indices);
00074 
00080   template<typename PointT>
00081   inline void
00082   getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);
00083 
00090   template<typename PointT>
00091   inline void
00092   getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, const Eigen::Vector4f &pivot_pt,
00093                   Eigen::Vector4f &max_pt);
00094 
00100   template <typename PointT> 
00101   inline void getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt);
00102   
00108   template <typename PointT> 
00109   inline void getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
00110 
00117   template <typename PointT> 
00118   inline void getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 
00119                            Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
00120 
00127   template <typename PointT> 
00128   inline void getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices, 
00129                            Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
00130 
00137   template <typename PointT> 
00138   inline double getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc);
00139 
00146   template <typename PointT> 
00147   inline void getMinMax (const PointT &histogram, int len, float &min_p, float &max_p);
00148 
00156   inline void 
00157   getMinMax (const sensor_msgs::PointCloud2 &cloud, int idx, const std::string &field_name, 
00158              float &min_p, float &max_p)
00159   {
00160     min_p = FLT_MAX;
00161     max_p = -FLT_MAX;
00162 
00163     int field_idx = pcl::getFieldIndex (cloud, field_name);
00164     if (field_idx == -1)
00165     {
00166       ROS_ERROR ("[getMinMax] Invalid field (%s) given!", field_name.c_str ());
00167       return;
00168     }
00169 
00170     for (unsigned int i = 0; i < cloud.fields[field_idx].count; ++i)
00171     {
00172       float data;
00173       // TODO: replace float with the real data type
00174       memcpy (&data, &cloud.data[cloud.fields[field_idx].offset + i * sizeof (float)], sizeof (float));
00175       min_p = (data > min_p) ? min_p : data; 
00176       max_p = (data < max_p) ? max_p : data; 
00177     }
00178   }
00179 
00185   inline void
00186   getMeanStdDev (const std::vector<float> &values, double &mean, double &stddev)
00187   {
00188     double sum = 0, sq_sum = 0;
00189 
00190     for (size_t i = 0; i < values.size (); ++i)
00191     {
00192       sum += values[i];
00193       sq_sum += values[i] * values[i];
00194     }
00195     mean = sum / values.size ();
00196     double variance = (double)(sq_sum - sum * sum / values.size ()) / (values.size () - 1);
00197     stddev = sqrt (variance);
00198   }
00199 
00200 }
00201 
00202 #include "pcl/common/common.hpp"
00203 
00204 #endif  //#ifndef PCL_COMMON_H_