一种点云快速法线估计方法

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 估计法线
int TopographyMesh::normalEstimation(
    std::vector<PointType>& in_points,
    std::vector<PCTTM_TriangleFace>& face_vect, 
    std::vector<Normal>& out_normals_face,
    std::vector<Normal>& out_normals_point,
    const int& k)
{
    if (in_points.size() <= 3)
    {
        std::cout << "pointVect <= 3 ..." << std::endl;
        return -1;
    }

    if (k<3)
    {
        std::cout << "'K' input must >= 3 ..." << std::endl;
        return -1;
    }
    int dataSize = in_points.size();

    std::vector<Vector3d> n_vect(face_vect.size());
    std::vector<PointType> mid_vect(face_vect.size());
    for (size_t i = 0; i < face_vect.size(); i++)
    {
        Vector3d v(
            in_points[face_vect[i].p_index_1].x - in_points[face_vect[i].p_index_0].x,
            in_points[face_vect[i].p_index_1].y - in_points[face_vect[i].p_index_0].y,
            in_points[face_vect[i].p_index_1].z - in_points[face_vect[i].p_index_0].z);
        Vector3d w(
            in_points[face_vect[i].p_index_2].x - in_points[face_vect[i].p_index_0].x,
            in_points[face_vect[i].p_index_2].y - in_points[face_vect[i].p_index_0].y,
            in_points[face_vect[i].p_index_2].z - in_points[face_vect[i].p_index_0].z);

         // Normal n(0.0, 0.0, 100.0);
         //法线方向矫正
         Eigen::Vector3d v1= v.cross(w);
         Eigen::Vector3d v2(0.0, 0.0, 100.0);
         if (atan2(v1.cross(v2).norm(), v1.transpose() * v2) > 0.5*M_PI)   //判断点的法线  与 固定法线的夹角
         {
             n_vect[i] = (-1)*v1;
         }
         else
         {
             n_vect[i] = v1;
         }
    
        //三角形几何中心
        mid_vect[i].x = (in_points[face_vect[i].p_index_2].x + in_points[face_vect[i].p_index_1].x + in_points[face_vect[i].p_index_0].x) / 3.0;
        mid_vect[i].y = (in_points[face_vect[i].p_index_2].y + in_points[face_vect[i].p_index_1].y + in_points[face_vect[i].p_index_0].y) / 3.0;
        mid_vect[i].z = (in_points[face_vect[i].p_index_2].z + in_points[face_vect[i].p_index_1].z + in_points[face_vect[i].p_index_0].z) / 3.0;
    }

    out_normals_face = n_vect;

    //kdtree “k临近”
    KDT::KDTree skdtree;
    skdtree.setNumOfLeafData(100);   //20-200  叶子存储数据数阈值
    skdtree.setInputPointCloud(mid_vect);
    skdtree.buildKDTree();

    out_normals_point.resize(dataSize);
    for (size_t j = 0; j < dataSize; j++)
    {
        std::vector<size_t> searchIndex(k);
        std::vector<float> searchDistance(k);
        skdtree.runKNNSearchK(in_points[j], k, &(searchIndex[0]), &(searchDistance[0]));
    
        Eigen::Vector3d xyz(0.0,0.0,0.0);
        for (size_t i = 0; i < k; i++)
        {
            xyz += n_vect[searchIndex[i]];
        }
        out_normals_point[j] = xyz / (1.0*k);
    }
    

    return 0;
}