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);
}
void find_feature_correspondence(
PointCloud<PFHSignature125>::Ptr &source_descriptors,
PointCloud<PFHSignature125>::Ptr &target_descriptors,
vector<int> &correspondence, vector<float> &correspondence_scores)
{
correspondence.resize(source_descriptors->size());
correspondence_scores.resize(source_descriptors->size());
pcl::KdTreeFLANN<pcl::PFHSignature125> descriptor_kdtree;
descriptor_kdtree.setInputCloud(target_descriptors);
const int k = 1;
vector<int> k_indices(k);
vector<float> k_squared_distances(k);
for (size_t i = 0; i < source_descriptors->size(); i++)
{
descriptor_kdtree.nearestKSearch(*source_descriptors, i, k,
k_indices, k_squared_distances);
correspondence[i] = k_indices[0];
correspondence_scores[i] = k_squared_distances[0];
cout << "step " << i << ": correspondence=" << correspondence[i] << endl;
cout << " correspondence_score=" << correspondence_scores[i] << endl;
}
};
void Observation<VertexType>::calcMeanCovThreadedAndCached(std::vector<VertexType* >& observation, SparseSurfaceAdjustmentParams& params){
if(!computedNeighbors){
std::vector<int> k_indices(params.normalExtractionMaxNeighbors);
std::vector<float> k_squared_distances(params.normalExtractionMaxNeighbors);
///allocating memory for neighbors caching
size_t pointCount = observation.size();
allocNeighborCache(pointCount, params.normalExtractionMaxNeighbors);
///create kD-Tree
PointTree kdTree;
kdTree.copyData(observation, true);
kdTree.createKDTree();
#pragma omp parallel for schedule(dynamic, 20) shared(observation, kdTree, params) firstprivate(k_indices, k_squared_distances)
for(unsigned int j = 0; j < observation.size(); ++j){
///FLANN neighbor search
neighbors_count_[j] = kdTree.radiusSearch(observation[j], params.normalExtractionMaxNeighborDistance, k_indices, k_squared_distances, params.normalExtractionMaxNeighbors);
//kdTree.nearestKSearch(observation[j], params.normalExtractionMaxNeighbors, k_indices, k_squared_distances);
///store neighbor pointer
for(size_t i = 0; i < neighbors_count_[j]; ++i){
neighbors_[j][i] = observation[k_indices[i]];
}
}
computedNeighbors = true;
}
double timing = g2o::get_time();
///calculate mean and covariance
PointVector mean;
VertexType* point = 0;
#pragma omp parallel for schedule(dynamic, 20) shared(observation) firstprivate(mean, point)
for(unsigned int j = 0; j < observation.size(); ++j){
point = observation[j];
if(neighbors_count_[j] >= (size_t) params.normalExtractionMinNeighbors){
Observation<VertexType>::getMean(neighbors_[j], neighbors_count_[j], mean);
PointMatrix covariance;
Observation<VertexType>::getCovariance(neighbors_[j], neighbors_count_[j], mean, covariance);
point->updateNormal(covariance);
} else {
point->covariance()= PointMatrix::Identity();
}
// point->_hasNormal = false;
}
std::cerr << "normal calculation took: " << (g2o::get_time() - timing) * 1000 << "ms." << std::endl;
}
void shot_detector::findCorrespondences(pcl::PointCloud<DescriptorType>::Ptr source, pcl::PointCloud<DescriptorType>::Ptr target, std::vector<int> &correspondences)
{
cout << "correspondence assignment..." << std::flush;
correspondences.resize (source->size());
// Use a KdTree to search for the nearest matches in feature space
pcl::KdTreeFLANN<DescriptorType> descriptor_kdtree;
descriptor_kdtree.setInputCloud (target);
// Find the index of the best match for each keypoint, and store it in "correspondences_out"
const int k = 1;
std::vector<int> k_indices (k);
std::vector<float> k_squared_distances (k);
for (int i = 0; i < static_cast<int> (source->size ()); ++i) {
descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
correspondences[i] = k_indices[0];
}
cout << "OK" << endl;
}
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);
}
