double findDistance(pcl::PointCloud<pcl::PointXYZRGB>::Ptr c1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr c2)
{
pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree->setInputCloud(c1);
int k = 1;
float min = -1.0;
std::vector<int> k_indices(1);
std::vector<float> k_distances(1);
for (int i = 0; i < c2->points.size(); i++)
{
tree->nearestKSearch(c2->points[i], k, k_indices, k_distances);
if (k_distances[0] < min || min == -1.0)
{
min = k_distances[0];
o1.x = c1->points[k_indices[0]].x;
o1.y = c1->points[k_indices[0]].y;
o1.z = c1->points[k_indices[0]].z;
o2.x = c2->points[i].x;
o2.y = c2->points[i].y;
o2.z = c2->points[i].z;
}
}
return sqrt(min);
}
template <typename PointInT> void
RejectivePointCloudCoherence<PointInT>::computeSampledCoherence (
const PointCloudInConstPtr &cloud, const Eigen::Affine3f &trans, float &w)
{
// std::unordered_multimap<int, int> index_map;
// index_map.reserve (cloud->size ());
// std::map <int,double> min_w_map;
std::map <int, int> target_model_map;
//Compute Half of Hausdorff distance
std::vector<int> k_indices(1);
std::vector<float> k_distances(1);
float max_dist = -1.0f * std::numeric_limits<float>::max ();
float sum_max_dist = 0.0f;
double val = 1.0;
int interval = cloud->points.size () / num_samples_;
PointInT transformed_pt;
//Now search for transformed_pt instead
//Main problem - need to pass transform
//float r1=0,r2=0, g1=0,g2=0,b1=0,b2=0;
if (interval < 2) //Special case - use every point, no random
{
for (size_t i = 0; i < cloud->points.size (); i++)
{
transformed_pt.getVector3fMap () = trans * cloud->points[i].getVector3fMap ();
transformed_pt.rgb = cloud->points[i].rgb;
search_->nearestKSearch (transformed_pt, 1, k_indices, k_distances);
k_distances[0] = sqrt (k_distances[0]);
if (k_distances[0] > max_dist)
max_dist = k_distances[0];
if (k_distances[0] < maximum_distance_)
val += computeScoreHSV (target_input_->points[k_indices[0]], transformed_pt, k_distances[0]);
sum_max_dist += k_distances[0];
}
}
else //otherwise randomly sample at regular intervals
{
boost::uniform_int<int> index_dist(0, interval);
int max_offset = cloud->points.size () - interval;
for (size_t offset = 0; offset < max_offset; offset += interval)
{
int idx = offset + index_dist(rng_);
transformed_pt.getVector3fMap () = trans * cloud->points[idx].getVector3fMap ();
transformed_pt.rgb = cloud->points[idx].rgb;
search_->nearestKSearch (transformed_pt, 1, k_indices, k_distances);
k_distances[0] = sqrt (k_distances[0]);
if (k_distances[0] > max_dist)
max_dist = k_distances[0];
if (k_distances[0] < maximum_distance_)
val += computeScoreHSV (target_input_->points[k_indices[0]], transformed_pt, k_distances[0]);
sum_max_dist += k_distances[0];
}
}
//Max distance allowed - coherence is zero if ANY points are too far away
if (max_dist > 0.05f)
{
// std::cout <<"REJECTING MAX\n";
w = -1.0f * std::numeric_limits<float>::min ();
return;
}
//Avereage max distance of points - coherence is zero if average is too large
float avg_max_dist = sqrt(sum_max_dist) / cloud->points.size ();
if (avg_max_dist > 0.02f)
{
// std::cout <<"REJECTING AVG\n";
w = -1.0f * std::numeric_limits<float>::min ();
return;
}
w = - static_cast<float> (val);
}
