TEST (PCL, KdTreeFLANN_setPointRepresentation)
{
PointCloud<MyPoint>::Ptr random_cloud (new PointCloud<MyPoint> ());
random_cloud->points.push_back (MyPoint (86.6f, 42.1f, 92.4f));
random_cloud->points.push_back (MyPoint (63.1f, 18.4f, 22.3f));
random_cloud->points.push_back (MyPoint (35.5f, 72.5f, 37.3f));
random_cloud->points.push_back (MyPoint (99.7f, 37.0f, 8.7f));
random_cloud->points.push_back (MyPoint (22.4f, 84.1f, 64.0f));
random_cloud->points.push_back (MyPoint (65.2f, 73.4f, 18.0f));
random_cloud->points.push_back (MyPoint (60.4f, 57.1f, 4.5f));
random_cloud->points.push_back (MyPoint (38.7f, 17.6f, 72.3f));
random_cloud->points.push_back (MyPoint (14.2f, 95.7f, 34.7f));
random_cloud->points.push_back (MyPoint ( 2.5f, 26.5f, 66.0f));
KdTreeFLANN<MyPoint> kdtree;
kdtree.setInputCloud (random_cloud);
MyPoint p (50.0f, 50.0f, 50.0f);
// Find k nearest neighbors
const int k = 10;
vector<int> k_indices (k);
vector<float> k_distances (k);
kdtree.nearestKSearch (p, k, k_indices, k_distances);
for (int i = 0; i < k; ++i)
{
// Compare to ground truth values, computed independently
static const int gt_indices[10] = {2, 7, 5, 1, 4, 6, 9, 0, 8, 3};
static const float gt_distances[10] =
{877.8f, 1674.7f, 1802.6f, 1937.5f, 2120.6f, 2228.8f, 3064.5f, 3199.7f, 3604.2f, 4344.8f};
EXPECT_EQ (k_indices[i], gt_indices[i]);
EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1);
}
// Find k nearest neighbors with a different point representation
boost::shared_ptr<MyPointRepresentationXY> ptrep (new MyPointRepresentationXY);
kdtree.setPointRepresentation (ptrep);
kdtree.nearestKSearch (p, k, k_indices, k_distances);
for (int i = 0; i < k; ++i)
{
// Compare to ground truth values, computed independently
static const int gt_indices[10] = {6, 2, 5, 1, 7, 0, 4, 3, 9, 8};
static const float gt_distances[10] =
{158.6f, 716.5f, 778.6f, 1170.2f, 1177.5f, 1402.0f, 1924.6f, 2639.1f, 2808.5f, 3370.1f};
EXPECT_EQ (k_indices[i], gt_indices[i]);
EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1);
}
// Go back to the default, this time with the values rescaled
DefaultPointRepresentation<MyPoint> point_rep;
float alpha[3] = {1.0f, 2.0f, 3.0f};
point_rep.setRescaleValues(alpha);
kdtree.setPointRepresentation (point_rep.makeShared ());
kdtree.nearestKSearch (p, k, k_indices, k_distances);
for (int i = 0; i < k; ++i)
{
// Compare to ground truth values, computed independently
static const int gt_indices[10] = {2, 9, 4, 7, 1, 5, 8, 0, 3, 6};
static const float gt_distances[10] =
{3686.9f, 6769.2f, 7177.0f, 8802.3f, 11071.5f, 11637.3f, 11742.4f, 17769.0f, 18497.3f, 18942.0f};
EXPECT_EQ (k_indices[i], gt_indices[i]);
EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1);
}
}
