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;
}
