66 Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg);
79 return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm ();
91 sqrPointToLineDistance (
const Eigen::Vector4f &pt,
const Eigen::Vector4f &line_pt,
const Eigen::Vector4f &line_dir,
const double sqr_length)
95 return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length;
105 template <
typename Po
intT>
double inline
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;
113 for (std::size_t i = 0; i < cloud.
size (); ++i)
115 for (std::size_t j = i; j < cloud.
size (); ++j)
118 double dist = (cloud[i].getVector4fMap () -
119 cloud[j].getVector4fMap ()).squaredNorm ();
120 if (dist <= max_dist)
129 if (i_min == token || i_max == token)
130 return (max_dist = std::numeric_limits<double>::min ());
134 return (std::sqrt (max_dist));
145 template <
typename Po
intT>
double inline
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;
153 for (std::size_t i = 0; i < indices.size (); ++i)
155 for (std::size_t j = i; j < indices.size (); ++j)
158 double dist = (cloud[indices[i]].getVector4fMap () -
159 cloud[indices[j]].getVector4fMap ()).squaredNorm ();
160 if (dist <= max_dist)
169 if (i_min == token || i_max == token)
170 return (max_dist = std::numeric_limits<double>::min ());
172 pmin = cloud[indices[i_min]];
173 pmax = cloud[indices[i_max]];
174 return (std::sqrt (max_dist));
181 template<
typename Po
intType1,
typename Po
intType2>
inline float
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);
192 template<>
inline float
195 float diff_x = p2.
x - p1.
x, diff_y = p2.
y - p1.
y;
196 return (diff_x*diff_x + diff_y*diff_y);
203 template<
typename Po
intType1,
typename Po
intType2>
inline float