vector<Correspondence3D> KeypointsCorrespondenceProjector::findLidarCorrespondences()
{
Stopwatch stopwatch;
stopwatch.restart();
cv::flann::KDTreeIndexParams index_params(1);
Mat source_image_keypoints = getTrainingPointsFromImageMatches(SOURCE);
cv::flann::Index source_kdTree(source_image_keypoints, index_params);
Mat target_image_keypoints = getTrainingPointsFromImageMatches(TARGET);
cv::flann::Index target_kdTree(target_image_keypoints, index_params);
Mat source_query = createQueryFromProjectedCloud(source_projection);
Mat source_indicies(source_query.rows, 1, CV_32SC1);
Mat source_distances(source_query.rows, 1, CV_32FC1);
source_kdTree.knnSearch(source_query, source_indicies, source_distances, 1);
/*draw3DTo2DMatches(source_image,
source_projection,
images_correspondences,
SOURCE,
source_indicies);*/
Mat target_query = createQueryFromProjectedCloud(target_projection);
Mat target_indicies(target_query.rows, 1, CV_32SC1);
Mat target_distances(target_query.rows, 1, CV_32FC1);
target_kdTree.knnSearch(target_query, target_indicies, target_distances, 1);
/*draw3DTo2DMatches(target_image,
target_projection,
images_correspondences,
TARGET,
target_indicies);*/
cerr << "Nearest projected points found in: " << stopwatch.elapsed() << "[sec]" << endl;
stopwatch.restart();
vector<Correspondence3D> final_correspondences = mergeCorrespondences(source_indicies,
source_distances,
target_indicies,
target_distances);
cerr << "Merged in: " << stopwatch.elapsed() << "[sec]" << endl;
cerr << "Correspondences projected: " << final_correspondences.size();
return final_correspondences;
}
void BinaryIndex::_initIndex(const int image_id, const std::vector<cv::KeyPoint>& kps, const cv::Mat& descs)
{
// Copying the descriptors to the merged matrix
_copyDescriptors(descs);
// Creating the feature index with the given descriptors.
flann::HierarchicalClusteringIndexParams index_params(_branching, flann::FLANN_CENTERS_RANDOM, _trees, _leaf_size);
FMat init_mat = _toFlannMat(_descriptors.rowRange(0, descs.rows));
_feat_index = new flann::Index< flann::Hamming<unsigned char> >(init_mat, index_params);
_feat_index->buildIndex();
// Associate these descriptors to the image in the inverse index.
for (size_t desc_ind = 0; desc_ind < _feat_index->size(); desc_ind++)
{
InvertedIndexEntry entry;
entry.image_id = image_id;
entry.distance = 0.0f;
entry.coords = kps[desc_ind].pt;
entry.orig_feat_id = desc_ind;
_inv_index[desc_ind].push_back(entry);
}
}
