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;
22 auto const& pt_i = *(*i);
23 auto const& pt_j = *(*j);
25 (pt_i[1] - pt_j[1]) * (pt_i[2] + pt_j[2]);
27 (pt_i[2] - pt_j[2]) * (pt_i[0] + pt_j[0]);
29 (pt_i[0] - pt_j[0]) * (pt_i[1] + pt_j[1]);
31 centroid += pt_j.asEigenVector3d();
34 plane_normal.normalize();
35 auto n_pnts(std::distance(pnts_begin, pnts_end));
40 d = centroid.dot(plane_normal) / n_pnts;
42 return std::make_pair(plane_normal, d);
45template <
class T_POINT>
47 const std::vector<T_POINT*>& pnts)
52template <
class T_POINT>
54 const std::vector<T_POINT>& pnts)
56 Eigen::Vector3d plane_normal({0, 0, 0});
57 Eigen::Vector3d centroid({0, 0, 0});
58 std::size_t n_pnts(pnts.size());
59 for (std::size_t i = n_pnts - 1, j = 0; j < n_pnts; i = j, j++)
61 plane_normal[0] += (pnts[i][1] - pnts[j][1]) *
62 (pnts[i][2] + pnts[j][2]);
63 plane_normal[1] += (pnts[i][2] - pnts[j][2]) *
64 (pnts[i][0] + pnts[j][0]);
65 plane_normal[2] += (pnts[i][0] - pnts[j][0]) *
66 (pnts[i][1] + pnts[j][1]);
68 centroid += pnts[j].asEigenVector3d();
71 plane_normal.normalize();
72 double const d = centroid.dot(plane_normal) / n_pnts;
73 return std::make_pair(plane_normal, d);
76template <
typename InputIterator>
77void rotatePoints(Eigen::Matrix3d
const& rot_mat, InputIterator pnts_begin,
78 InputIterator pnts_end)
80 for (
auto it = pnts_begin; it != pnts_end; ++it)
82 (*it)->asEigenVector3d() = rot_mat * (*it)->asEigenVector3d();
86template <
typename InputIterator1,
typename InputIterator2>
88 InputIterator1 p_pnts_end,
89 InputIterator2 r_pnts_begin,
90 InputIterator2 r_pnts_end)
92 assert(std::distance(p_pnts_begin, p_pnts_end) > 2);
95 auto const [plane_normal, d] =
102 for (
auto it = r_pnts_begin; it != r_pnts_end; ++it)
111void 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)