Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
distances.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <Eigen/Core>
44
45#include <algorithm>
46#include <cstring>
47#include <vector>
48
49namespace pcl {
50namespace distances {
51
52/** \brief Compute the median value from a set of doubles
53 * \param[in] fvec the set of doubles
54 * \param[in] m the number of doubles in the set
55 */
56inline double
57computeMedian(double* fvec, int m)
58{
59 // Copy the values to vectors for faster sorting
60 std::vector<double> data(fvec, fvec + m);
61
62 std::nth_element(data.begin(), data.begin() + (data.size() >> 1), data.end());
63 return (data[data.size() >> 1]);
64}
65
66/** \brief Use a Huber kernel to estimate the distance between two vectors
67 * \param[in] p_src the first eigen vector
68 * \param[in] p_tgt the second eigen vector
69 * \param[in] sigma the sigma value
70 */
71inline double
72huber(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt, double sigma)
73{
74 Eigen::Array4f diff = (p_tgt.array() - p_src.array()).abs();
75 double norm = 0.0;
76 for (int i = 0; i < 3; ++i) {
77 if (diff[i] < sigma)
78 norm += diff[i] * diff[i];
79 else
80 norm += 2.0 * sigma * diff[i] - sigma * sigma;
81 }
82 return (norm);
83}
84
85/** \brief Use a Huber kernel to estimate the distance between two vectors
86 * \param[in] diff the norm difference between two vectors
87 * \param[in] sigma the sigma value
88 */
89inline double
90huber(double diff, double sigma)
91{
92 double norm = 0.0;
93 if (diff < sigma)
94 norm += diff * diff;
95 else
96 norm += 2.0 * sigma * diff - sigma * sigma;
97 return (norm);
98}
99
100/** \brief Use a Gedikli kernel to estimate the distance between two vectors
101 * (for more information, see
102 * \param[in] val the norm difference between two vectors
103 * \param[in] clipping the clipping value
104 * \param[in] slope the slope. Default: 4
105 */
106inline double
107gedikli(double val, double clipping, double slope = 4)
108{
109 return (1.0 / (1.0 + pow(std::abs(val) / clipping, slope)));
110}
111
112/** \brief Compute the Manhattan distance between two eigen vectors.
113 * \param[in] p_src the first eigen vector
114 * \param[in] p_tgt the second eigen vector
115 */
116inline double
117l1(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
118{
119 return ((p_src.array() - p_tgt.array()).abs().sum());
120}
121
122/** \brief Compute the Euclidean distance between two eigen vectors.
123 * \param[in] p_src the first eigen vector
124 * \param[in] p_tgt the second eigen vector
125 */
126inline double
127l2(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
128{
129 return ((p_src - p_tgt).norm());
130}
131
132/** \brief Compute the squared Euclidean distance between two eigen vectors.
133 * \param[in] p_src the first eigen vector
134 * \param[in] p_tgt the second eigen vector
135 */
136inline double
137l2Sqr(const Eigen::Vector4f& p_src, const Eigen::Vector4f& p_tgt)
138{
139 return ((p_src - p_tgt).squaredNorm());
140}
141} // namespace distances
142} // namespace pcl
double l2Sqr(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the squared Euclidean distance between two eigen vectors.
Definition distances.h:137
double gedikli(double val, double clipping, double slope=4)
Use a Gedikli kernel to estimate the distance between two vectors (for more information,...
Definition distances.h:107
double huber(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt, double sigma)
Use a Huber kernel to estimate the distance between two vectors.
Definition distances.h:72
double computeMedian(double *fvec, int m)
Compute the median value from a set of doubles.
Definition distances.h:57
double l2(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Euclidean distance between two eigen vectors.
Definition distances.h:127
double l1(const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt)
Compute the Manhattan distance between two eigen vectors.
Definition distances.h:117