格子点クラウド推定法の高速法線

////////////////////////////////////////////////// ////////////////////////////////////////////////// //////////////////////////////// // 
//は通常の推定
int型TopographyMesh :: normalEstimationを（
    std ::ベクトル <点>＆in_points、
    std ::ベクトル <PCTTM_TriangleFace>＆face_vect、
    std ::ベクトル <通常>＆out_normals_face、
    std ::ベクトル <通常>＆out_normals_point、
     constの int型＆K）
{
    もし（in_points.size（）<= 3 ）
    {
        std :: coutの << " pointVect <= 3 ... " << はstd ::てendl;
        リターン - 1 。
    }

    もし（K < 3 ）
    {
        std :: coutの << " 'K'入力必須> = 3 ... " << はstd ::てendl;
        リターン - 1 。
    }
    INT DATASIZE = in_points.size（）。

    std ::ベクトル <のVector3D> n_vect（face_vect.size（））;
    std ::ベクトル <点> mid_vect（face_vect.size（））;
    以下のための（iは= SIZE_T 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（
            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）。

         // 通常N-（0.0、0.0、100.0）;
          // 法線方向補正 
         固有、V1 = ::のVector3D v.cross（W）。
         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。
         }
         そうしないと
         {
             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（データサイズ）。
    用は（size_t J = 0 ; J <データサイズ、J ++ ）
    {
        std ::ベクトル <size_tの> にSearchIndex（K）。
        std ::ベクトル < フロート > searchDistance（K）。
        skdtree.runKNNSearchK（in_points [J]、K、 ＆（にSearchIndex [ 0 ]）、＆（searchDistance [ 0 ]））;
    
        固有::のVector3D XYZ（0.0、0.0、0.0 ）。
        以下のための（iは= size_tの0を、I <K; I ++ ）
        {
            XYZ + = n_vect〔にSearchIndex [I]]。
        }
        out_normals_point [J] = XYZ /（1.0 * K）。
    }
    

    リターン 0 ;
}