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, 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 */ 00038 00039 #ifndef PCL_COMMON_NORMS_IMPL_HPP_ 00040 #define PCL_COMMON_NORMS_IMPL_HPP_ 00041 00042 #include <pcl/pcl_macros.h> 00043 #include <pcl/console/print.h> 00044 00045 namespace pcl 00046 { 00047 00048 template <typename FloatVectorT> inline float 00049 selectNorm (FloatVectorT a, FloatVectorT b, int dim, NormType norm_type) 00050 { 00051 // {L1, L2_SQR, L2, LINF, JM, B, SUBLINEAR, CS, DIV, PF, K, KL, HIK}; 00052 switch (norm_type) 00053 { 00054 case (L1): 00055 return L1_Norm (a, b, dim); 00056 case (L2_SQR): 00057 return L2_Norm_SQR (a, b, dim); 00058 case (L2): 00059 return L2_Norm (a, b, dim); 00060 case (LINF): 00061 return Linf_Norm (a, b, dim); 00062 case (JM): 00063 return JM_Norm (a, b, dim); 00064 case (B): 00065 return B_Norm (a, b, dim); 00066 case (SUBLINEAR): 00067 return Sublinear_Norm (a, b, dim); 00068 case (CS): 00069 return CS_Norm (a, b, dim); 00070 case (DIV): 00071 return Div_Norm (a, b, dim); 00072 case (KL): 00073 return KL_Norm (a, b, dim); 00074 case (HIK): 00075 return HIK_Norm (a, b, dim); 00076 00077 case (PF): 00078 case (K): 00079 default: 00080 PCL_ERROR ("[pcl::selectNorm] For PF and K norms you have to explicitly call the method, as they need additional parameters\n"); 00081 return -1; 00082 } 00083 } 00084 00085 00086 template <typename FloatVectorT> inline float 00087 L1_Norm (FloatVectorT a, FloatVectorT b, int dim) 00088 { 00089 float norm = 0.0f; 00090 for (int i = 0; i < dim; ++i) 00091 norm += fabsf(a[i] - b[i]); 00092 return norm; 00093 } 00094 00095 00096 template <typename FloatVectorT> inline float 00097 L2_Norm_SQR (FloatVectorT a, FloatVectorT b, int dim) 00098 { 00099 float norm = 0.0; 00100 for (int i = 0; i < dim; ++i) 00101 { 00102 float diff = a[i] - b[i]; 00103 norm += diff*diff; 00104 } 00105 return norm; 00106 } 00107 00108 00109 template <typename FloatVectorT> inline float 00110 L2_Norm (FloatVectorT a, FloatVectorT b, int dim) 00111 { 00112 return sqrtf(L2_Norm_SQR(a, b, dim)); 00113 } 00114 00115 00116 template <typename FloatVectorT> inline float 00117 Linf_Norm (FloatVectorT a, FloatVectorT b, int dim) 00118 { 00119 float norm = 0.0; 00120 for (int i = 0; i < dim; ++i) 00121 norm = (std::max)(fabsf(a[i] - b[i]), norm); 00122 return norm; 00123 } 00124 00125 00126 template <typename FloatVectorT> inline float 00127 JM_Norm (FloatVectorT a, FloatVectorT b, int dim) 00128 { 00129 float norm = 0.0; 00130 00131 for (int i = 0; i < dim; ++i) 00132 norm += (sqrtf (a[i]) - sqrtf (b[i])) * (sqrtf (a[i]) - sqrtf (b[i])); 00133 00134 return sqrtf (norm); 00135 } 00136 00137 00138 template <typename FloatVectorT> inline float 00139 B_Norm (FloatVectorT a, FloatVectorT b, int dim) 00140 { 00141 float norm = 0.0, result; 00142 00143 for (int i = 0; i < dim; ++i) 00144 norm += sqrtf (a[i] * b[i]); 00145 00146 if (norm > 0) 00147 result = -logf (norm); 00148 else 00149 result = 0; 00150 00151 return result; 00152 } 00153 00154 00155 template <typename FloatVectorT> inline float 00156 Sublinear_Norm (FloatVectorT a, FloatVectorT b, int dim) 00157 { 00158 float norm = 0.0; 00159 00160 for (int i = 0; i < dim; ++i) 00161 norm += sqrtf (fabsf (a[i] - b[i])); 00162 00163 return norm; 00164 } 00165 00166 00167 template <typename FloatVectorT> inline float 00168 CS_Norm (FloatVectorT a, FloatVectorT b, int dim) 00169 { 00170 float norm = 0.0; 00171 00172 for (int i = 0; i < dim; ++i) 00173 if ((a[i] + b[i]) != 0) 00174 norm += (a[i] - b[i]) * (a[i] - b[i]) / (a[i] + b[i]); 00175 else 00176 norm += 0; 00177 return norm; 00178 } 00179 00180 00181 template <typename FloatVectorT> inline float 00182 Div_Norm (FloatVectorT a, FloatVectorT b, int dim) 00183 { 00184 float norm = 0.0; 00185 00186 for (int i = 0; i < dim; ++i) 00187 if ((a[i] / b[i]) > 0) 00188 norm += (a[i] - b[i]) * logf (a[i] / b[i]); 00189 else 00190 norm += 0; 00191 return norm; 00192 } 00193 00194 00195 template <typename FloatVectorT> inline float 00196 PF_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2) 00197 { 00198 float norm = 0.0; 00199 00200 for (int i = 0; i < dim; ++i) 00201 norm += (P1 * a[i] - P2 * b[i]) * (P1 * a[i] - P2 * b[i]); 00202 return sqrtf (norm); 00203 } 00204 00205 00206 template <typename FloatVectorT> inline float 00207 K_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2) 00208 { 00209 float norm = 0.0; 00210 00211 for (int i = 0; i < dim; ++i) 00212 norm += fabsf (P1 * a[i] - P2 * b[i]); 00213 return norm; 00214 } 00215 00216 00217 template <typename FloatVectorT> inline float 00218 KL_Norm (FloatVectorT a, FloatVectorT b, int dim) 00219 { 00220 float norm = 0.0; 00221 00222 for (int i = 0; i < dim; ++i) 00223 if ( (b[i] != 0) && ((a[i] / b[i]) > 0) ) 00224 norm += a[i] * logf (a[i] / b[i]); 00225 else 00226 norm += 0; 00227 return norm; 00228 } 00229 00230 00231 template <typename FloatVectorT> inline float 00232 HIK_Norm(FloatVectorT a, FloatVectorT b, int dim) 00233 { 00234 float norm = 0.0f; 00235 for (int i = 0; i < dim; ++i) 00236 norm += (std::min)(a[i], b[i]); 00237 return norm; 00238 } 00239 00240 } 00241 #endif 00242