使用 nanoflann 进行最近邻搜索

Question

我正在使用 nanoflann to do nearest neighbor search by following the example given here。

基本上，我在 Eigen::MatrixXf 中存储了一个点云，其形状为 (#points, 7)，其中每一行包含 x、y、z , intensity, r, g, b 个点的值。我想搜索 cloud 它的 k 最近邻居中的每个点，然后将 indices 和 dists 分别存储在两个 Eigen::Matrix 中。这是我的代码：

void searchNN(const Eigen::MatrixXf & cloud, const size_t k, Eigen::MatrixXi &indices, Eigen::MatrixXf &dists)
{
    // Eigen::MatrixXf uses colMajor as default
    // copy the coords to a RowMajor matrix and search in this matrix
    // the nearest neighbors for each datapoint
    Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> coords = cloud.leftCols(3);

    // different max_leaf values only affect the search speed 
    // and any value between 10 - 50 is reasonable
    const int max_leaf = 10;
    nanoflann::KDTreeEigenMatrixAdaptor<Eigen::MatrixXf> mat_index(coords, max_leaf);
    mat_index.index->buildIndex();
    indices.resize(cloud.rows(), k);
    dists.resize(cloud.rows(), k);
    // do a knn search
    for (int i = 0; i < coords.rows(); ++i) {
        // coords is RowMajor so coords.data()[i*3+0 / +1  / +2] represents the ith row of coords
        std::vector<float> query_pt{ coords.data()[i*3+0], coords.data()[i*3+1], coords.data()[i*3+2] };

        std::vector<size_t> ret_indices(k);
        std::vector<float> out_dists_sqr(k);
        nanoflann::KNNResultSet<float> resultSet(k);
        resultSet.init(&ret_indices[0], &out_dists_sqr[0]);
        mat_index.index->findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
        for (size_t j = 0; j < k; ++j) {
            indices(i, j) = ret_indices[j];
            dists(i, j) = std::sqrt(out_dists_sqr[j]);
        }
    }
}

我的问题是：搜索结果错误。 indices矩阵的行是相同的，即对于cloud中的每个点，knn都是相同的。但是既然云中的点不同，怎么会有相同的nn呢？我的代码中肯定有问题，但我可以找到它。如果您能帮助我更正它，我将不胜感激。例如，cloud 中的前五个点是：

3.165   3.681   -2.669  -1550  79   87   100
-6.614  -4.137  0.465   -1376  169  172  189
1.012   -2.032  0.767   -1753  246  244  247
0.974   3.197   -2.923  -1432  81   80   96
-2.353  -1.323  -1.535  -1162  122  120  99

而indices的前五行是：

193 194 195 196 197 198 199 187 188 189
193 194 195 196 197 198 199 187 188 189
193 194 195 196 197 198 199 187 188 189
193 194 195 196 197 198 199 187 188 189
193 194 195 196 197 198 199 187 188 189

（我只有 200 点云用于测试目的。）

Answer 1

您KDTreeEigenMatrixAdaptor定义不正确（类型不匹配）：

typedef Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> RowMatX3f;
RowMatX3f coords = cloud.leftCols(3);
nanoflann::KDTreeEigenMatrixAdaptor<RowMatX3f> mat_index(3, coords, max_leaf);

使用 nanoflann 进行最近邻搜索

nearest neighbors search with nanoflann

c++

knn

point-clouds

eigen

flann