Point Cloud Library (PCL)  1.14.1-dev
distances.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #pragma once
39 
40 #include <limits>
41 
42 #include <pcl/types.h>
43 #include <pcl/point_types.h> // for PointXY
44 #include <Eigen/Core> // for VectorXf
45 
46 /**
47  * \file pcl/common/distances.h
48  * Define standard C methods to do distance calculations
49  * \ingroup common
50  */
51 
52 /*@{*/
53 namespace pcl
54 {
55  template <typename PointT> class PointCloud;
56 
57  /** \brief Get the shortest 3D segment between two 3D lines
58  * \param line_a the coefficients of the first line (point, direction)
59  * \param line_b the coefficients of the second line (point, direction)
60  * \param pt1_seg the first point on the line segment
61  * \param pt2_seg the second point on the line segment
62  * \ingroup common
63  */
64  PCL_EXPORTS void
65  lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b,
66  Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg);
67 
68  /** \brief Get the square distance from a point to a line (represented by a point and a direction)
69  * \param pt a point
70  * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
71  * \param line_dir the line direction
72  * \ingroup common
73  */
74  double inline
75  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
76  {
77  // Calculate the distance from the point to the line
78  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
79  return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm ();
80  }
81 
82  /** \brief Get the square distance from a point to a line (represented by a point and a direction)
83  * \note This one is useful if one has to compute many distances to a fixed line, so the vector length can be pre-computed
84  * \param pt a point
85  * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
86  * \param line_dir the line direction
87  * \param sqr_length the squared norm of the line direction
88  * \ingroup common
89  */
90  double inline
91  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, const double sqr_length)
92  {
93  // Calculate the distance from the point to the line
94  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
95  return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length;
96  }
97 
98  /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
99  * \param[in] cloud the point cloud dataset
100  * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
101  * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
102  * \return the length of segment length
103  * \ingroup common
104  */
105  template <typename PointT> double inline
107  PointT &pmin, PointT &pmax)
108  {
109  double max_dist = std::numeric_limits<double>::min ();
110  const auto token = std::numeric_limits<std::size_t>::max();
111  std::size_t i_min = token, i_max = token;
112 
113  for (std::size_t i = 0; i < cloud.size (); ++i)
114  {
115  for (std::size_t j = i; j < cloud.size (); ++j)
116  {
117  // Compute the distance
118  double dist = (cloud[i].getVector4fMap () -
119  cloud[j].getVector4fMap ()).squaredNorm ();
120  if (dist <= max_dist)
121  continue;
122 
123  max_dist = dist;
124  i_min = i;
125  i_max = j;
126  }
127  }
128 
129  if (i_min == token || i_max == token)
130  return (max_dist = std::numeric_limits<double>::min ());
131 
132  pmin = cloud[i_min];
133  pmax = cloud[i_max];
134  return (std::sqrt (max_dist));
135  }
136 
137  /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
138  * \param[in] cloud the point cloud dataset
139  * \param[in] indices a set of point indices to use from \a cloud
140  * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
141  * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
142  * \return the length of segment length
143  * \ingroup common
144  */
145  template <typename PointT> double inline
146  getMaxSegment (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
147  PointT &pmin, PointT &pmax)
148  {
149  double max_dist = std::numeric_limits<double>::min ();
150  const auto token = std::numeric_limits<std::size_t>::max();
151  std::size_t i_min = token, i_max = token;
152 
153  for (std::size_t i = 0; i < indices.size (); ++i)
154  {
155  for (std::size_t j = i; j < indices.size (); ++j)
156  {
157  // Compute the distance
158  double dist = (cloud[indices[i]].getVector4fMap () -
159  cloud[indices[j]].getVector4fMap ()).squaredNorm ();
160  if (dist <= max_dist)
161  continue;
162 
163  max_dist = dist;
164  i_min = i;
165  i_max = j;
166  }
167  }
168 
169  if (i_min == token || i_max == token)
170  return (max_dist = std::numeric_limits<double>::min ());
171 
172  pmin = cloud[indices[i_min]];
173  pmax = cloud[indices[i_max]];
174  return (std::sqrt (max_dist));
175  }
176 
177  /** \brief Calculate the squared euclidean distance between the two given points.
178  * \param[in] p1 the first point
179  * \param[in] p2 the second point
180  */
181  template<typename PointType1, typename PointType2> inline float
182  squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
183  {
184  float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
185  return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
186  }
187 
188  /** \brief Calculate the squared euclidean distance between the two given points.
189  * \param[in] p1 the first point
190  * \param[in] p2 the second point
191  */
192  template<> inline float
193  squaredEuclideanDistance (const PointXY& p1, const PointXY& p2)
194  {
195  float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y;
196  return (diff_x*diff_x + diff_y*diff_y);
197  }
198 
199  /** \brief Calculate the euclidean distance between the two given points.
200  * \param[in] p1 the first point
201  * \param[in] p2 the second point
202  */
203  template<typename PointType1, typename PointType2> inline float
204  euclideanDistance (const PointType1& p1, const PointType2& p2)
205  {
206  return (std::sqrt (squaredEuclideanDistance (p1, p2)));
207  }
208 }
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
Defines all the PCL implemented PointT point type structures.
double getMaxSegment(const pcl::PointCloud< PointT > &cloud, PointT &pmin, PointT &pmax)
Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
Definition: distances.h:106
PCL_EXPORTS void lineToLineSegment(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
Get the shortest 3D segment between two 3D lines.
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction)
Definition: distances.h:75
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:182
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:204
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Defines basic non-point types used by PCL.