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.)
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);