KPConv cpp_wrappers c++ 代码

为什么总是出错

已于 2024-08-11 14:59:42 修改

阅读量231

点赞数 3

文章标签： c++ 算法开发语言

于 2024-08-11 14:25:15 首次发布

本文链接：https://blog.csdn.net/m0_55630410/article/details/141105079

版权

关于 KPConv 内Python使用的C++扩展代码

这两个工具的C++部分代码解析

cpp_neighbors

实际上在C++内使用的只是一个函数：

void batch_nanoflann_neighbors( vector<PointXYZ>& queries, vector<PointXYZ>& supports, vector<int>& q_batches, vector<int>& s_batches,
                                vector<int>& neighbors_indices, float radius ) {

    // Initialize variables
    // ******************

    // indices
    int i0 = 0;

    // Square radius
    float r2 = radius * radius;

    // Counting vector
    int                                 max_count = 0;
    float                               d2;
    vector<vector<pair<size_t, float>>> all_inds_dists( queries.size() );

    // batch index
    int b      = 0;
    int sum_qb = 0;
    int sum_sb = 0;

    // Nanoflann related variables
    // ***************************

    // CLoud variable
    PointCloud current_cloud;

    // Tree parameters
    nanoflann::KDTreeSingleIndexAdaptorParams tree_params( 10 /* max leaf */ );

    // KDTree type definition
    typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, PointCloud>, PointCloud, 3> my_kd_tree_t;

    // Pointer to trees
    my_kd_tree_t* index;

    // Build KDTree for the first batch element
    current_cloud.pts = vector<PointXYZ>( supports.begin() + sum_sb, supports.begin() + sum_sb + s_batches[ b ] );
    index             = new my_kd_tree_t( 3, current_cloud, tree_params );
    index->buildIndex();

    // Search neigbors indices
    // ***********************

    // Search params
    nanoflann::SearchParams search_params;
    search_params.sorted = true;

    for ( auto& p0 : queries ) {

        // Check if we changed batch
        if ( i0 == sum_qb + q_batches[ b ] ) {
            sum_qb += q_batches[ b ];
            sum_sb += s_batches[ b ];
            b++;

            // Change the points
            current_cloud.pts.clear();
            current_cloud.pts = vector<PointXYZ>( supports.begin() + sum_sb, supports.begin() + sum_sb + s_batches[ b ] );

            // Build KDTree of the current element of the batch
            delete index;
            index = new my_kd_tree_t( 3, current_cloud, tree_params );
            index->buildIndex();
        }

        // Initial guess of neighbors size
        all_inds_dists[ i0 ].reserve( max_count );

        // Find neighbors
        float  query_pt[ 3 ] = { p0.x, p0.y, p0.z };
        size_t nMatches      = index->radiusSearch( query_pt, r2, all_inds_dists[ i0 ], search_params );

        // Update max count
        if ( nMatches > max_count )
            max_count = nMatches;

        // Increment query idx
        i0++;
    }

    // Reserve the memory
    neighbors_indices.resize( queries.size() * max_count );
    i0     = 0;
    sum_sb = 0;
    sum_qb = 0;
    b      = 0;
    for ( auto& inds_dists : all_inds_dists ) {
        // Check if we changed batch
        if ( i0 == sum_qb + q_batches[ b ] ) {
            sum_qb += q_batches[ b ];
            sum_sb += s_batches[ b ];
            b++;
        }

        for ( int j = 0; j < max_count; j++ ) {
            if ( j < inds_dists.size() )
                neighbors_indices[ i0 * max_count + j ] = inds_dists[ j ].first + sum_sb;
            else
                neighbors_indices[ i0 * max_count + j ] = supports.size();
        }
        i0++;
    }

    delete index;

    return;
}

实际上这个函数的功能就是搜索xyz点云数组内每个点的邻近点，
假设输入数组为：Nx3 ，
这时候执行邻近点搜索，但是，每个点的邻近点数量是不一致的，
在执行邻近搜索时，会得到每个点的邻近点个数，那么就以这个每个邻近点个数的最大值最为输出的第二个维度，如果最大邻近点个数表示为L，即最后的结果就是 NxL。
可以看到：

for ( int j = 0; j < max_count; j++ ) {
    if ( j < inds_dists.size() )
        neighbors_indices[ i0 * max_count + j ] = inds_dists[ j ].first + sum_sb;
    else
        neighbors_indices[ i0 * max_count + j ] = supports.size();
}

这里就是在填充邻近点索引，max_count就是得到的最大邻近点个数，假如实际搜索到的邻近点个数为 5，那么，在max_count个索引数组内，前5个就是得到的邻近点索引，而后面的都用supports.size填充了。