| 1 |
/** |
| 2 |
* @file rrts02-nearestNeighbors.cpp |
| 3 |
* @author Can Erdogan |
| 4 |
* @date Feb 04, 2013 |
| 5 |
* @brief Checks if the nearest neighbor computation done by flann is correct. |
| 6 |
*/ |
| 7 |
|
| 8 |
#include <iostream> |
| 9 |
#include <Eigen/Core> |
| 10 |
#include <dart/dart.hpp> |
| 11 |
#include <gtest/gtest.h> |
| 12 |
#if HAVE_FLANN |
| 13 |
# include <flann/flann.hpp> |
| 14 |
#endif // HAVE_FLANN |
| 15 |
#include "TestHelpers.hpp" |
| 16 |
|
| 17 |
/* ********************************************************************************************* |
| 18 |
*/ |
| 19 |
#if HAVE_FLANN |
| 20 |
TEST(NEAREST_NEIGHBOR, 2D) |
| 21 |
{ |
| 22 |
|
| 23 |
// Build the index with the first node |
| 24 |
flann::Index<flann::L2<double> > index( |
| 25 |
flann::KDTreeSingleIndexParams(10, true)); |
| 26 |
Eigen::VectorXd p1(2); |
| 27 |
p1 << -3.04159, -3.04159; |
| 28 |
index.buildIndex(flann::Matrix<double>((double*)p1.data(), 1, p1.size())); |
| 29 |
|
| 30 |
// Add two more points |
| 31 |
Eigen::Vector2d p2(-2.96751, -2.97443), p3(-2.91946, -2.88672); |
| 32 |
index.addPoints(flann::Matrix<double>((double*)p2.data(), 1, p2.size())); |
| 33 |
index.addPoints(flann::Matrix<double>((double*)p3.data(), 1, p3.size())); |
| 34 |
|
| 35 |
// Check the size of the tree |
| 36 |
EXPECT_EQ(3, (int)index.size()); |
| 37 |
|
| 38 |
// Get the nearest neighbor index for a sample point |
| 39 |
Eigen::Vector2d sample(-2.26654, 2.2874); |
| 40 |
int nearest; |
| 41 |
double distance; |
| 42 |
const flann::Matrix<double> queryMatrix( |
| 43 |
(double*)sample.data(), 1, sample.size()); |
| 44 |
flann::Matrix<int> nearestMatrix(&nearest, 1, 1); |
| 45 |
flann::Matrix<double> distanceMatrix(flann::Matrix<double>(&distance, 1, 1)); |
| 46 |
index.knnSearch( |
| 47 |
queryMatrix, |
| 48 |
nearestMatrix, |
| 49 |
distanceMatrix, |
| 50 |
1, |
| 51 |
flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED)); |
| 52 |
EXPECT_EQ(2, nearest); |
| 53 |
|
| 54 |
// Get the nearest neighbor |
| 55 |
double* point = index.getPoint(nearest); |
| 56 |
bool equality = equals(Vector2d(point[0], point[1]), p3, 1e-3); |
| 57 |
EXPECT_TRUE(equality); |
| 58 |
} |
| 59 |
#endif // HAVE_FLANN |