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) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 #ifndef PCL_REGISTRATION_DISTANCES_H 00041 #define PCL_REGISTRATION_DISTANCES_H 00042 00043 #include <pcl/registration/eigen.h> 00044 #include <vector> 00045 00046 namespace pcl 00047 { 00048 namespace distances 00049 { 00050 00051 /** \brief Compute the median value from a set of doubles 00052 * \param[in] fvec the set of doubles 00053 * \param[in] m the number of doubles in the set 00054 */ 00055 inline double 00056 computeMedian (double *fvec, int m) 00057 { 00058 // Copy the values to vectors for faster sorting 00059 std::vector<double> data (m); 00060 memcpy (&data[0], fvec, sizeof (double) * m); 00061 00062 std::nth_element(data.begin(), data.begin() + (data.size () >> 1), data.end()); 00063 return (data[data.size () >> 1]); 00064 } 00065 00066 /** \brief Use a Huber kernel to estimate the distance between two vectors 00067 * \param[in] p_src the first eigen vector 00068 * \param[in] p_tgt the second eigen vector 00069 * \param[in] sigma the sigma value 00070 */ 00071 inline double 00072 huber (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt, double sigma) 00073 { 00074 Eigen::Array4f diff = (p_tgt.array () - p_src.array ()).abs (); 00075 double norm = 0.0; 00076 for (int i = 0; i < 3; ++i) 00077 { 00078 if (diff[i] < sigma) 00079 norm += diff[i] * diff[i]; 00080 else 00081 norm += 2.0 * sigma * diff[i] - sigma * sigma; 00082 } 00083 return (norm); 00084 } 00085 00086 /** \brief Use a Huber kernel to estimate the distance between two vectors 00087 * \param[in] diff the norm difference between two vectors 00088 * \param[in] sigma the sigma value 00089 */ 00090 inline double 00091 huber (double diff, double sigma) 00092 { 00093 double norm = 0.0; 00094 if (diff < sigma) 00095 norm += diff * diff; 00096 else 00097 norm += 2.0 * sigma * diff - sigma * sigma; 00098 return (norm); 00099 } 00100 00101 /** \brief Use a Gedikli kernel to estimate the distance between two vectors 00102 * (for more information, see 00103 * \param[in] val the norm difference between two vectors 00104 * \param[in] clipping the clipping value 00105 * \param[in] slope the slope. Default: 4 00106 */ 00107 inline double 00108 gedikli (double val, double clipping, double slope = 4) 00109 { 00110 return (1.0 / (1.0 + pow (fabs(val) / clipping, slope))); 00111 } 00112 00113 /** \brief Compute the Manhattan distance between two eigen vectors. 00114 * \param[in] p_src the first eigen vector 00115 * \param[in] p_tgt the second eigen vector 00116 */ 00117 inline double 00118 l1 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 00119 { 00120 return ((p_src.array () - p_tgt.array ()).abs ().sum ()); 00121 } 00122 00123 /** \brief Compute the Euclidean distance between two eigen vectors. 00124 * \param[in] p_src the first eigen vector 00125 * \param[in] p_tgt the second eigen vector 00126 */ 00127 inline double 00128 l2 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 00129 { 00130 return ((p_src - p_tgt).norm ()); 00131 } 00132 00133 /** \brief Compute the squared Euclidean distance between two eigen vectors. 00134 * \param[in] p_src the first eigen vector 00135 * \param[in] p_tgt the second eigen vector 00136 */ 00137 inline double 00138 l2Sqr (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) 00139 { 00140 return ((p_src - p_tgt).squaredNorm ()); 00141 } 00142 } 00143 } 00144 00145 #endif