最近的邻居用nanoflann搜索

Question

我使用nanoflann按照这里给出的示例进行最近邻居搜索。

基本上，我在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);