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