Point Cloud Library (PCL)  1.14.1-dev
norms.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 #include <pcl/common/norms.h>
42 #include <pcl/console/print.h>
43 #include <pcl/pcl_macros.h>
44 
45 
46 namespace pcl
47 {
48 
49 template <typename FloatVectorT> inline float
50 selectNorm (FloatVectorT a, FloatVectorT b, int dim, NormType norm_type)
51 {
52  // {L1, L2_SQR, L2, LINF, JM, B, SUBLINEAR, CS, DIV, PF, K, KL, HIK};
53  switch (norm_type)
54  {
55  case (L1):
56  return L1_Norm (a, b, dim);
57  case (L2_SQR):
58  return L2_Norm_SQR (a, b, dim);
59  case (L2):
60  return L2_Norm (a, b, dim);
61  case (LINF):
62  return Linf_Norm (a, b, dim);
63  case (JM):
64  return JM_Norm (a, b, dim);
65  case (B):
66  return B_Norm (a, b, dim);
67  case (SUBLINEAR):
68  return Sublinear_Norm (a, b, dim);
69  case (CS):
70  return CS_Norm (a, b, dim);
71  case (DIV):
72  return Div_Norm (a, b, dim);
73  case (KL):
74  return KL_Norm (a, b, dim);
75  case (HIK):
76  return HIK_Norm (a, b, dim);
77 
78  case (PF):
79  case (K):
80  default:
81  PCL_ERROR ("[pcl::selectNorm] For PF and K norms you have to explicitly call the method, as they need additional parameters\n");
82  return -1;
83  }
84 }
85 
86 
87 template <typename FloatVectorT> inline float
88 L1_Norm (FloatVectorT a, FloatVectorT b, int dim)
89 {
90  float norm = 0.0f;
91  for (int i = 0; i < dim; ++i)
92  norm += std::abs(a[i] - b[i]);
93  return norm;
94 }
95 
96 
97 template <typename FloatVectorT> inline float
98 L2_Norm_SQR (FloatVectorT a, FloatVectorT b, int dim)
99 {
100  float norm = 0.0;
101  for (int i = 0; i < dim; ++i)
102  {
103  float diff = a[i] - b[i];
104  norm += diff*diff;
105  }
106  return norm;
107 }
108 
109 
110 template <typename FloatVectorT> inline float
111 L2_Norm (FloatVectorT a, FloatVectorT b, int dim)
112 {
113  return std::sqrt (L2_Norm_SQR(a, b, dim));
114 }
115 
116 
117 template <typename FloatVectorT> inline float
118 Linf_Norm (FloatVectorT a, FloatVectorT b, int dim)
119 {
120  float norm = 0.0;
121  for (int i = 0; i < dim; ++i)
122  norm = (std::max)(std::abs(a[i] - b[i]), norm);
123  return norm;
124 }
125 
126 
127 template <typename FloatVectorT> inline float
128 JM_Norm (FloatVectorT a, FloatVectorT b, int dim)
129 {
130  float norm = 0.0;
131 
132  for (int i = 0; i < dim; ++i)
133  norm += (std::sqrt (a[i]) - std::sqrt (b[i])) * (std::sqrt (a[i]) - std::sqrt (b[i]));
134 
135  return std::sqrt (norm);
136 }
137 
138 
139 template <typename FloatVectorT> inline float
140 B_Norm (FloatVectorT a, FloatVectorT b, int dim)
141 {
142  float norm = 0.0, result;
143 
144  for (int i = 0; i < dim; ++i)
145  norm += std::sqrt (a[i] * b[i]);
146 
147  if (norm > 0)
148  result = -std::log (norm);
149  else
150  result = 0;
151 
152  return result;
153 }
154 
155 
156 template <typename FloatVectorT> inline float
157 Sublinear_Norm (FloatVectorT a, FloatVectorT b, int dim)
158 {
159  float norm = 0.0;
160 
161  for (int i = 0; i < dim; ++i)
162  norm += std::sqrt (std::abs (a[i] - b[i]));
163 
164  return norm;
165 }
166 
167 
168 template <typename FloatVectorT> inline float
169 CS_Norm (FloatVectorT a, FloatVectorT b, int dim)
170 {
171  float norm = 0.0;
172 
173  for (int i = 0; i < dim; ++i)
174  if ((a[i] + b[i]) != 0)
175  norm += (a[i] - b[i]) * (a[i] - b[i]) / (a[i] + b[i]);
176  else
177  norm += 0;
178  return norm;
179 }
180 
181 
182 template <typename FloatVectorT> inline float
183 Div_Norm (FloatVectorT a, FloatVectorT b, int dim)
184 {
185  float norm = 0.0;
186 
187  for (int i = 0; i < dim; ++i)
188  if ((a[i] / b[i]) > 0)
189  norm += (a[i] - b[i]) * std::log (a[i] / b[i]);
190  else
191  norm += 0;
192  return norm;
193 }
194 
195 
196 template <typename FloatVectorT> inline float
197 PF_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2)
198 {
199  float norm = 0.0;
200 
201  for (int i = 0; i < dim; ++i)
202  norm += (P1 * a[i] - P2 * b[i]) * (P1 * a[i] - P2 * b[i]);
203  return std::sqrt (norm);
204 }
205 
206 
207 template <typename FloatVectorT> inline float
208 K_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2)
209 {
210  float norm = 0.0;
211 
212  for (int i = 0; i < dim; ++i)
213  norm += std::abs (P1 * a[i] - P2 * b[i]);
214  return norm;
215 }
216 
217 
218 template <typename FloatVectorT> inline float
219 KL_Norm (FloatVectorT a, FloatVectorT b, int dim)
220 {
221  float norm = 0.0;
222 
223  for (int i = 0; i < dim; ++i)
224  if ( (b[i] != 0) && ((a[i] / b[i]) > 0) )
225  norm += a[i] * std::log (a[i] / b[i]);
226  else
227  norm += 0;
228  return norm;
229 }
230 
231 
232 template <typename FloatVectorT> inline float
233 HIK_Norm(FloatVectorT a, FloatVectorT b, int dim)
234 {
235  float norm = 0.0f;
236  for (int i = 0; i < dim; ++i)
237  norm += (std::min)(a[i], b[i]);
238  return norm;
239 }
240 
241 } // namespace pcl
242 
float selectNorm(FloatVectorT a, FloatVectorT b, int dim, NormType norm_type)
Method that calculates any norm type available, based on the norm_type variable.
Definition: norms.hpp:50
float B_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the B norm of the vector between two points.
Definition: norms.hpp:140
float KL_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the KL between two discrete probability density functions.
Definition: norms.hpp:219
float JM_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the JM norm of the vector between two points.
Definition: norms.hpp:128
float K_Norm(FloatVectorT a, FloatVectorT b, int dim, float P1, float P2)
Compute the K norm of the vector between two points.
Definition: norms.hpp:208
float L1_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the L1 norm of the vector between two points.
Definition: norms.hpp:88
float Linf_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the L-infinity norm of the vector between two points.
Definition: norms.hpp:118
float L2_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the L2 norm of the vector between two points.
Definition: norms.hpp:111
float CS_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the CS norm of the vector between two points.
Definition: norms.hpp:169
float HIK_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the HIK norm of the vector between two points.
Definition: norms.hpp:233
NormType
Enum that defines all the types of norms available.
Definition: norms.h:54
float Sublinear_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the sublinear norm of the vector between two points.
Definition: norms.hpp:157
float Div_Norm(FloatVectorT a, FloatVectorT b, int dim)
Compute the div norm of the vector between two points.
Definition: norms.hpp:183
float L2_Norm_SQR(FloatVectorT a, FloatVectorT b, int dim)
Compute the squared L2 norm of the vector between two points.
Definition: norms.hpp:98
float PF_Norm(FloatVectorT a, FloatVectorT b, int dim, float P1, float P2)
Compute the PF norm of the vector between two points.
Definition: norms.hpp:197
@ HIK
Definition: norms.h:54
@ SUBLINEAR
Definition: norms.h:54
@ PF
Definition: norms.h:54
@ K
Definition: norms.h:54
@ LINF
Definition: norms.h:54
@ L2
Definition: norms.h:54
@ DIV
Definition: norms.h:54
@ KL
Definition: norms.h:54
@ L2_SQR
Definition: norms.h:54
@ B
Definition: norms.h:54
@ L1
Definition: norms.h:54
@ CS
Definition: norms.h:54
@ JM
Definition: norms.h:54
Define standard C methods to calculate different norms.
Defines all the PCL and non-PCL macros used.