c++ - 最近的邻居用nanoflann搜索

Question

我正在通过遵循给定的nanoflann的示例，使用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都相同。但是，由于云中的点是不同的，因此它们如何具有相同的nns？我的代码中肯定有错误，但是我可以找到它。非常感谢您能帮助我更正此问题。
例如，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);