13template <
typename InputIterator>
15 InputIterator pnts_end)
17 Eigen::Vector3d plane_normal({0, 0, 0});
18 Eigen::Vector3d centroid({0, 0, 0});
19 for (
auto i=std::prev(pnts_end), j=pnts_begin; j!=pnts_end; i = j, ++j) {
20 auto const& pt_i = *(*i);
21 auto const& pt_j = *(*j);
22 plane_normal[0] += (pt_i[1] - pt_j[1])
23 * (pt_i[2] + pt_j[2]);
24 plane_normal[1] += (pt_i[2] - pt_j[2])
25 * (pt_i[0] + pt_j[0]);
26 plane_normal[2] += (pt_i[0] - pt_j[0])
27 * (pt_i[1] + pt_j[1]);
29 centroid += pt_j.asEigenVector3d();
32 plane_normal.normalize();
33 auto n_pnts(std::distance(pnts_begin, pnts_end));
38 d = centroid.dot(plane_normal) / n_pnts;
40 return std::make_pair(plane_normal, d);
43template <
class T_POINT>
45 const std::vector<T_POINT*>& pnts)
50template <
class T_POINT>
52 const std::vector<T_POINT>& pnts)
54 Eigen::Vector3d plane_normal({0, 0, 0});
55 Eigen::Vector3d centroid({0, 0, 0});
56 std::size_t n_pnts(pnts.size());
57 for (std::size_t i = n_pnts - 1, j = 0; j < n_pnts; i = j, j++) {
58 plane_normal[0] += (pnts[i][1] - pnts[j][1])
59 * (pnts[i][2] + pnts[j][2]);
60 plane_normal[1] += (pnts[i][2] - pnts[j][2])
61 * (pnts[i][0] + pnts[j][0]);
62 plane_normal[2] += (pnts[i][0] - pnts[j][0])
63 * (pnts[i][1] + pnts[j][1]);
65 centroid += pnts[j].asEigenVector3d();
68 plane_normal.normalize();
69 double const d = centroid.dot(plane_normal) / n_pnts;
70 return std::make_pair(plane_normal, d);
74template <
typename InputIterator>
75void rotatePoints(Eigen::Matrix3d
const& rot_mat, InputIterator pnts_begin,
76 InputIterator pnts_end)
78 for (
auto it = pnts_begin; it != pnts_end; ++it)
80 (*it)->asEigenVector3d() = rot_mat * (*it)->asEigenVector3d();
84template <
typename InputIterator1,
typename InputIterator2>
86 InputIterator1 p_pnts_end,
87 InputIterator2 r_pnts_begin,
88 InputIterator2 r_pnts_end)
90 assert(std::distance(p_pnts_begin, p_pnts_end) > 2);
93 auto const [plane_normal, d] =
100 for (
auto it = r_pnts_begin; it != r_pnts_end; ++it)
109void rotatePoints(Eigen::Matrix3d
const& rot_mat, std::vector<P*>
const& pnts)
void rotatePoints(Eigen::Matrix3d const &rot_mat, InputIterator pnts_begin, InputIterator pnts_end)
Eigen::Matrix3d computeRotationMatrixToXY(Eigen::Vector3d const &n)
Eigen::Matrix3d rotatePointsToXY(InputIterator1 p_pnts_begin, InputIterator1 p_pnts_end, InputIterator2 r_pnts_begin, InputIterator2 r_pnts_end)
std::pair< Eigen::Vector3d, double > getNewellPlane(InputIterator pnts_begin, InputIterator pnts_end)