使用 nanoflann 进行最近邻搜索
nearest neighbors search with nanoflann
我正在使用 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 点云用于测试目的。)
您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 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 点云用于测试目的。)
您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);