Point Cloud Library (PCL) 1.12.1
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/*@{*/
53namespace 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
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.