int main (int argc, char** argv)
{
int nbs=1;
float rad = 10;
vector<int> indices(nbs);//temp vector to hold indices of the nearest neighbours found
vector<float> sqr_distances(nbs);//temp vector to hold the distances to the nearest neighbours
int total_num_found=0;
int total_sociable=0;
PointCloud<PointT>::Ptr scan1(new PointCloud<PointT>);
PointCloud<PointT>::Ptr scan2(new PointCloud<PointT>);
io::loadPCDFile<PointT>(argv[1], *scan1);
io::loadPCDFile<PointT>(argv[2], *scan2);
KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud(scan2);
float distance=0;
int num_sociable=0;
for(int j=0; j<scan1->points.size(); j++)
{
int num_found = kdtree.nearestKSearch((scan1->points)[j], 1, indices, sqr_distances);
if(num_found > 0)
{
num_sociable++;
distance += sqr_distances[0];
}
}
float rmsdistance = sqrt((float)distance/num_sociable);
cout << "RMS distance: " << rmsdistance << endl;
}
double
computeRmsE (const PointCloud<PointType>::ConstPtr &model, const PointCloud<PointType>::ConstPtr &scene, const Eigen::Matrix4f &rototranslation)
{
PointCloud<PointType> transformed_model;
transformPointCloud (*model, transformed_model, rototranslation);
KdTreeFLANN<PointType> tree;
tree.setInputCloud (scene);
double sqr_norm_sum = 0;
int found_points = 0;
vector<int> neigh_indices (1);
vector<float> neigh_sqr_dists (1);
for (size_t i = 0; i < transformed_model.size (); ++i)
{
int found_neighs = tree.nearestKSearch (transformed_model.at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1)
{
++found_points;
sqr_norm_sum += static_cast<double> (neigh_sqr_dists[0]);
}
}
if (found_points > 0)
return sqrt (sqr_norm_sum / double (transformed_model.size ()));
return numeric_limits<double>::max ();
}
TEST (PCL, KdTreeFLANN_radiusSearch)
{
KdTreeFLANN<MyPoint> kdtree;
kdtree.setInputCloud (cloud.makeShared ());
MyPoint test_point(0.0f, 0.0f, 0.0f);
double max_dist = 0.15;
set<int> brute_force_result;
for (unsigned int i=0; i<cloud.points.size(); ++i)
if (euclideanDistance(cloud.points[i], test_point) < max_dist)
brute_force_result.insert(i);
vector<int> k_indices;
vector<float> k_distances;
kdtree.radiusSearch (test_point, max_dist, k_indices, k_distances, 100);
//cout << k_indices.size()<<"=="<<brute_force_result.size()<<"?\n";
for (size_t i = 0; i < k_indices.size (); ++i)
{
set<int>::iterator brute_force_result_it = brute_force_result.find (k_indices[i]);
bool ok = brute_force_result_it != brute_force_result.end ();
//if (!ok) cerr << k_indices[i] << " is not correct...\n";
//else cerr << k_indices[i] << " is correct...\n";
EXPECT_EQ (ok, true);
if (ok)
brute_force_result.erase (brute_force_result_it);
}
//for (set<int>::const_iterator it=brute_force_result.begin(); it!=brute_force_result.end(); ++it)
//cerr << "FLANN missed "<<*it<<"\n";
bool error = brute_force_result.size () > 0;
//if (error) cerr << "Missed too many neighbors!\n";
EXPECT_EQ (error, false);
{
KdTreeFLANN<MyPoint> kdtree;
kdtree.setInputCloud (cloud_big.makeShared ());
// preallocate indices and dists arrays
k_indices.resize(cloud_big.points.size ());
k_distances.resize(cloud_big.points.size ());
ScopeTime scopeTime ("FLANN radiusSearch");
{
for (size_t i = 0; i < cloud_big.points.size (); ++i)
kdtree.radiusSearch (cloud_big.points[i], 0.1, k_indices, k_distances);
}
}
{
KdTreeFLANN<MyPoint> kdtree (false);
kdtree.setInputCloud (cloud_big.makeShared ());
// preallocate indices and dists arrays
k_indices.resize(cloud_big.points.size ());
k_distances.resize(cloud_big.points.size ());
ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)");
{
for (size_t i = 0; i < cloud_big.points.size (); ++i)
kdtree.radiusSearch (cloud_big.points[i], 0.1, k_indices, k_distances);
}
}
}
TEST (PCL, KdTreeFLANN_radiusSearchEigen)
{
KdTreeFLANN<Eigen::MatrixXf> kdtree;
kdtree.setInputCloud (cloud_eigen.makeShared ());
kdtree.setInputCloud (cloud_eigen.makeShared ());
Eigen::VectorXf test_point = Eigen::Vector3f (0.0f, 0.0f, 0.0f);
double max_dist = 0.15;
set<int> brute_force_result;
for (int i = 0; i < cloud_eigen.points.rows (); ++i)
if ((cloud_eigen.points.row (i) - test_point.transpose ()).norm () < max_dist)
brute_force_result.insert (i);
vector<int> k_indices;
vector<float> k_distances;
kdtree.radiusSearch (test_point, max_dist, k_indices, k_distances);
for (size_t i = 0; i < k_indices.size (); ++i)
{
set<int>::iterator brute_force_result_it = brute_force_result.find (k_indices[i]);
bool ok = brute_force_result_it != brute_force_result.end ();
EXPECT_EQ (ok, true);
if (ok)
brute_force_result.erase (brute_force_result_it);
}
bool error = brute_force_result.size () > 0;
EXPECT_EQ (error, false);
{
KdTreeFLANN<Eigen::MatrixXf> kdtree;
kdtree.setInputCloud (cloud_eigen.makeShared ());
// preallocate indices and dists arrays
k_indices.resize (cloud_eigen.points.rows ());
k_distances.resize (cloud_eigen.points.rows ());
ScopeTime scopeTime ("FLANN radiusSearch");
{
for (int i = 0; i < cloud_eigen.points.rows (); ++i)
kdtree.radiusSearch (cloud_eigen.points.row (i), 0.1, k_indices, k_distances);
}
}
{
KdTreeFLANN<Eigen::MatrixXf> kdtree (false);
kdtree.setInputCloud (cloud_eigen.makeShared ());
// preallocate indices and dists arrays
k_indices.resize (cloud_eigen.points.rows ());
k_distances.resize (cloud_eigen.points.rows ());
ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)");
{
for (int i = 0; i < cloud_eigen.points.rows (); ++i)
kdtree.radiusSearch (cloud_eigen.points.row (i), 0.1, k_indices, k_distances);
}
}
}
bool isInStableArea(std::vector<XYZNormalCloud::Ptr> &clouds,
std::vector< KdTreeFLANN<PointNormalT>::Ptr > &trees,
PointNormalT searchPoint,
std::vector<int> neighborhood_scale)
{
if(!(clouds.size() == trees.size() && trees.size() == neighborhood_scale.size())) {
std::cout << "The sizes of the vectors does not agree." << endl;
return false;
}
std::vector<float> curvature_variances;
std::vector<float> curvature_means;
for(int i = 0; i < clouds.size();i++) {
//Looks for (neighborhhod_scale[i]) neighbours of pointSearch at point cloud i
KdTreeFLANN<PointNormalT> kdtree = *(trees.at(i));
vector<int> pointIdxNKNSearch(neighborhood_scale[i]);
vector<float> pointNKNSquaredDistance(neighborhood_scale[i]);
int neighbours = kdtree.nearestKSearch (searchPoint, neighborhood_scale[i], pointIdxNKNSearch, pointNKNSquaredDistance);
if(neighbours > 0) { //If there is a vicinity....
//warns if the nbeighbourhood is smaller than expected
if(neighbours < neighborhood_scale[i])
std::cout << "Less points then expected..." << std::endl;
//copy the curvature values of each neighbour point and calculates its mean and variance..
std::vector<float> local_curvatures(neighbours);
for(int j = 0; j < pointIdxNKNSearch.size();j++) {
int pointIdx = pointIdxNKNSearch.at(j);
local_curvatures.push_back(clouds.at(i)->at(pointIdx).curvature);
}
float mean,var;
calculate_statistics(local_curvatures,mean,var);
curvature_means.push_back(mean); curvature_variances.push_back(var);
}
}
float mean,var;
calculate_statistics(curvature_variances,mean,var);
return false;
}
TEST (PCL, KdTreeFLANN_32_vs_64_bit)
{
KdTreeFLANN<PointXYZ> tree;
tree.setInputCloud (cloud_in);
std::vector<std::vector<int> > nn_indices_vector;
for (size_t i = 0; i < cloud_in->size (); ++i)
if (isFinite ((*cloud_in)[i]))
{
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree.radiusSearch ((*cloud_in)[i], 0.02, nn_indices, nn_dists);
nn_indices_vector.push_back (nn_indices);
}
for (size_t vec_i = 0; vec_i < nn_indices_vector.size (); ++vec_i)
{
char str[512];
sprintf (str, "point_%d", int (vec_i));
boost::optional<boost::property_tree::ptree&> tree = xml_property_tree.get_child_optional (str);
if (!tree)
FAIL ();
int vec_size = tree.get ().get<int> ("size");
EXPECT_EQ (vec_size, nn_indices_vector[vec_i].size ());
for (size_t n_i = 0; n_i < nn_indices_vector[vec_i].size (); ++n_i)
{
sprintf (str, "nn_%d", int (n_i));
int neighbor_index = tree.get ().get<int> (str);
EXPECT_EQ (neighbor_index, nn_indices_vector[vec_i][n_i]);
}
}
}
TEST (PCL, KdTreeFLANN_nearestKSearchEigen)
{
KdTreeFLANN<Eigen::MatrixXf> kdtree;
kdtree.setInputCloud (cloud_eigen.makeShared ());
Eigen::VectorXf test_point = Eigen::Vector3f (0.01f, 0.01f, 0.01f);
unsigned int no_of_neighbors = 20;
multimap<float, int> sorted_brute_force_result;
for (int i = 0; i < cloud_eigen.points.rows (); ++i)
{
float distance = (cloud_eigen.points.row (i) - test_point.transpose ()).norm ();
sorted_brute_force_result.insert (make_pair (distance, static_cast<int> (i)));
}
float max_dist = 0.0f;
unsigned int counter = 0;
for (multimap<float, int>::iterator it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end () && counter < no_of_neighbors; ++it)
{
max_dist = max (max_dist, it->first);
++counter;
}
vector<int> k_indices;
k_indices.resize (no_of_neighbors);
vector<float> k_distances;
k_distances.resize (no_of_neighbors);
kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
//if (k_indices.size() != no_of_neighbors) cerr << "Found "<<k_indices.size()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
EXPECT_EQ (k_indices.size (), no_of_neighbors);
// Check if all found neighbors have distance smaller than max_dist
for (size_t i = 0; i < k_indices.size (); ++i)
{
const Eigen::VectorXf& point = cloud_eigen.points.row (k_indices[i]);
bool ok = (test_point - point).norm () <= max_dist;
if (!ok)
ok = (fabs ((test_point - point).norm () - max_dist)) <= 1e-6;
//if (!ok) cerr << k_indices[i] << " is not correct...\n";
//else cerr << k_indices[i] << " is correct...\n";
EXPECT_EQ (ok, true);
}
ScopeTime scopeTime ("FLANN nearestKSearch");
{
KdTreeFLANN<Eigen::MatrixXf> kdtree;
kdtree.setInputCloud (cloud_eigen.makeShared ());
for (int i = 0; i < cloud_eigen.points.rows (); ++i)
kdtree.nearestKSearch (cloud_eigen.points.row (i), no_of_neighbors, k_indices, k_distances);
}
}
TEST (PCL, KdTreeFLANN_nearestKSearch)
{
KdTreeFLANN<MyPoint> kdtree;
kdtree.setInputCloud (cloud.makeShared ());
MyPoint test_point (0.01f, 0.01f, 0.01f);
unsigned int no_of_neighbors = 20;
multimap<float, int> sorted_brute_force_result;
for (size_t i = 0; i < cloud.points.size (); ++i)
{
float distance = euclideanDistance (cloud.points[i], test_point);
sorted_brute_force_result.insert (make_pair (distance, static_cast<int> (i)));
}
float max_dist = 0.0f;
unsigned int counter = 0;
for (multimap<float, int>::iterator it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end () && counter < no_of_neighbors; ++it)
{
max_dist = max (max_dist, it->first);
++counter;
}
vector<int> k_indices;
k_indices.resize (no_of_neighbors);
vector<float> k_distances;
k_distances.resize (no_of_neighbors);
kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
//if (k_indices.size() != no_of_neighbors) cerr << "Found "<<k_indices.size()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
EXPECT_EQ (k_indices.size (), no_of_neighbors);
// Check if all found neighbors have distance smaller than max_dist
for (size_t i = 0; i < k_indices.size (); ++i)
{
const MyPoint& point = cloud.points[k_indices[i]];
bool ok = euclideanDistance (test_point, point) <= max_dist;
if (!ok)
ok = (fabs (euclideanDistance (test_point, point)) - max_dist) <= 1e-6;
//if (!ok) cerr << k_indices[i] << " is not correct...\n";
//else cerr << k_indices[i] << " is correct...\n";
EXPECT_EQ (ok, true);
}
ScopeTime scopeTime ("FLANN nearestKSearch");
{
KdTreeFLANN<MyPoint> kdtree;
kdtree.setInputCloud (cloud_big.makeShared ());
for (size_t i = 0; i < cloud_big.points.size (); ++i)
kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
}
}
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);
}
}