42 #include <pcl/common/common.h>
62 Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg);
75 return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm ();
87 sqrPointToLineDistance (
const Eigen::Vector4f &pt,
const Eigen::Vector4f &line_pt,
const Eigen::Vector4f &line_dir,
const double sqr_length)
91 return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length;
101 template <
typename Po
intT>
double inline
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;
109 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
111 for (std::size_t j = i; j < cloud.
points.size (); ++j)
114 double dist = (cloud.
points[i].getVector4fMap () -
115 cloud.
points[j].getVector4fMap ()).squaredNorm ();
116 if (dist <= max_dist)
125 if (i_min == token || i_max == token)
126 return (max_dist = std::numeric_limits<double>::min ());
128 pmin = cloud.
points[i_min];
129 pmax = cloud.
points[i_max];
130 return (std::sqrt (max_dist));
141 template <
typename Po
intT>
double inline
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;
149 for (std::size_t i = 0; i < indices.size (); ++i)
151 for (std::size_t j = i; j < indices.size (); ++j)
154 double dist = (cloud.
points[indices[i]].getVector4fMap () -
155 cloud.
points[indices[j]].getVector4fMap ()).squaredNorm ();
156 if (dist <= max_dist)
165 if (i_min == token || i_max == token)
166 return (max_dist = std::numeric_limits<double>::min ());
168 pmin = cloud.
points[indices[i_min]];
169 pmax = cloud.
points[indices[i_max]];
170 return (std::sqrt (max_dist));
177 template<
typename Po
intType1,
typename Po
intType2>
inline float
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);
188 template<>
inline float
191 float diff_x = p2.
x - p1.
x, diff_y = p2.
y - p1.
y;
192 return (diff_x*diff_x + diff_y*diff_y);
199 template<
typename Po
intType1,
typename Po
intType2>
inline float
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
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.
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) ...
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.