c++eigenknnpoint-cloudsflann

nearest neighbors search with nanoflann


I am using nanoflann to do nearest neighbor search by following the example given here.

Basically, I have a point cloud stored in Eigen::MatrixXf with shape of (#points, 7) where each row contains the x, y, z, intensity, r, g, b values of a point. I want to search for each point in the cloud its k nearest neighbors and then store the indices and dists in two Eigen::Matrix respectivley. Here is my code:

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]);
        }
    }
}

My problem is: the searching result is wrong. The rows of indices matrix are identical, that is, for each point in the cloud, the knn are the same. But since the points in the cloud are different, how can they have the same nns? There are definitely something going wrong in my code but I can find it. I very appreciate if you could help me to correct it. For example, the first five points in the cloud are:

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

And the first five rows of indices are:

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

(I have just a 200 points cloud for testing purposes.)


Solution

  • You KDTreeEigenMatrixAdaptor is not properly defined (the types do not match):

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