Point Cloud Library (PCL)  1.9.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/common/common.h>
43 
44 /**
45  * \file pcl/common/distances.h
46  * Define standard C methods to do distance calculations
47  * \ingroup common
48  */
49 
50 /*@{*/
51 namespace pcl
52 {
53  /** \brief Get the shortest 3D segment between two 3D lines
54  * \param line_a the coefficients of the first line (point, direction)
55  * \param line_b the coefficients of the second line (point, direction)
56  * \param pt1_seg the first point on the line segment
57  * \param pt2_seg the second point on the line segment
58  * \ingroup common
59  */
60  PCL_EXPORTS void
61  lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b,
62  Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg);
63 
64  /** \brief Get the square distance from a point to a line (represented by a point and a direction)
65  * \param pt a point
66  * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
67  * \param line_dir the line direction
68  * \ingroup common
69  */
70  double inline
71  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
72  {
73  // Calculate the distance from the point to the line
74  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
75  return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm ();
76  }
77 
78  /** \brief Get the square distance from a point to a line (represented by a point and a direction)
79  * \note This one is useful if one has to compute many distances to a fixed line, so the vector length can be pre-computed
80  * \param pt a point
81  * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
82  * \param line_dir the line direction
83  * \param sqr_length the squared norm of the line direction
84  * \ingroup common
85  */
86  double inline
87  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, const double sqr_length)
88  {
89  // Calculate the distance from the point to the line
90  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
91  return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length;
92  }
93 
94  /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
95  * \param[in] cloud the point cloud dataset
96  * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
97  * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
98  * \return the length of segment length
99  * \ingroup common
100  */
101  template <typename PointT> double inline
103  PointT &pmin, PointT &pmax)
104  {
105  double max_dist = std::numeric_limits<double>::min ();
106  const auto token = std::numeric_limits<std::size_t>::max();
107  std::size_t i_min = token, i_max = token;
108 
109  for (std::size_t i = 0; i < cloud.points.size (); ++i)
110  {
111  for (std::size_t j = i; j < cloud.points.size (); ++j)
112  {
113  // Compute the distance
114  double dist = (cloud.points[i].getVector4fMap () -
115  cloud.points[j].getVector4fMap ()).squaredNorm ();
116  if (dist <= max_dist)
117  continue;
118 
119  max_dist = dist;
120  i_min = i;
121  i_max = j;
122  }
123  }
124 
125  if (i_min == token || i_max == token)
126  return (max_dist = std::numeric_limits<double>::min ());
127 
128  pmin = cloud.points[i_min];
129  pmax = cloud.points[i_max];
130  return (std::sqrt (max_dist));
131  }
132 
133  /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
134  * \param[in] cloud the point cloud dataset
135  * \param[in] indices a set of point indices to use from \a cloud
136  * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
137  * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
138  * \return the length of segment length
139  * \ingroup common
140  */
141  template <typename PointT> double inline
142  getMaxSegment (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
143  PointT &pmin, PointT &pmax)
144  {
145  double max_dist = std::numeric_limits<double>::min ();
146  const auto token = std::numeric_limits<std::size_t>::max();
147  std::size_t i_min = token, i_max = token;
148 
149  for (std::size_t i = 0; i < indices.size (); ++i)
150  {
151  for (std::size_t j = i; j < indices.size (); ++j)
152  {
153  // Compute the distance
154  double dist = (cloud.points[indices[i]].getVector4fMap () -
155  cloud.points[indices[j]].getVector4fMap ()).squaredNorm ();
156  if (dist <= max_dist)
157  continue;
158 
159  max_dist = dist;
160  i_min = i;
161  i_max = j;
162  }
163  }
164 
165  if (i_min == token || i_max == token)
166  return (max_dist = std::numeric_limits<double>::min ());
167 
168  pmin = cloud.points[indices[i_min]];
169  pmax = cloud.points[indices[i_max]];
170  return (std::sqrt (max_dist));
171  }
172 
173  /** \brief Calculate the squared euclidean distance between the two given points.
174  * \param[in] p1 the first point
175  * \param[in] p2 the second point
176  */
177  template<typename PointType1, typename PointType2> inline float
178  squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
179  {
180  float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
181  return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
182  }
183 
184  /** \brief Calculate the squared euclidean distance between the two given points.
185  * \param[in] p1 the first point
186  * \param[in] p2 the second point
187  */
188  template<> inline float
189  squaredEuclideanDistance (const PointXY& p1, const PointXY& p2)
190  {
191  float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y;
192  return (diff_x*diff_x + diff_y*diff_y);
193  }
194 
195  /** \brief Calculate the euclidean distance between the two given points.
196  * \param[in] p1 the first point
197  * \param[in] p2 the second point
198  */
199  template<typename PointType1, typename PointType2> inline float
200  euclideanDistance (const PointType1& p1, const PointType2& p2)
201  {
202  return (std::sqrt (squaredEuclideanDistance (p1, p2)));
203  }
204 }
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:102
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:394
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:178
A 2D point structure representing Euclidean xy coordinates.
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:200
Define standard C methods and C++ classes that are common to all methods.
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:71
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.
#define PCL_EXPORTS
Definition: pcl_macros.h:241