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