vocabtree  0.0.1
vision.cxx
Go to the documentation of this file.
1 #include "vision.hpp"
2 
3 #include <opencv2/nonfree/nonfree.hpp>
4 
5 namespace vision {
6 
7  void geo_verify_f(const cv::Mat &descriptors0, const cv::Mat &points0,
8  const cv::Mat &descriptors1, const cv::Mat &points1, cv::detail::MatchesInfo &matches_info,
9  std::vector<uint32_t> *inliers0, std::vector<uint32_t> *inliers1) {
10 
11  cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::KDTreeIndexParams>();
12  cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>();
13  matches_info.confidence = 0;
14  matches_info.num_inliers = 0;
15  matches_info.H = cv::Mat::zeros(cv::Size(3, 3), CV_32FC1);
16  if(descriptors1.rows < 9 || descriptors0.rows < 9 || descriptors0.cols < 128 || descriptors1.cols < 128) return;
17  // if (descriptors0.depth() == CV_8U) {
18  // indexParams->setAlgorithm(cvflann::FLANN_INDEX_LSH);
19  // searchParams->setAlgorithm(cvflann::FLANN_INDEX_LSH);
20  // }
21  try {
22  cv::FlannBasedMatcher matcher(indexParams, searchParams);
23  std::vector< std::vector<cv::DMatch> > pair_matches;
24  // std::cout << "knn match 1" << std::endl;
25  MatchesSet matches;
26  // Find 1->2 matches
27  cv::Mat descriptors0f, descriptors1f;
28  descriptors0.convertTo(descriptors0f, CV_32FC1);
29  descriptors1.convertTo(descriptors1f, CV_32FC1);
30  matcher.knnMatch(descriptors0f, descriptors1f, pair_matches, 2);
31  float match_conf_ = 0.2f;
32  for (size_t i = 0; i < pair_matches.size(); ++i) {
33  if (pair_matches[i].size() < 2) continue;
34  const cv::DMatch& m0 = pair_matches[i][0];
35  const cv::DMatch& m1 = pair_matches[i][1];
36  if (m0.distance < (1.f - match_conf_) * m1.distance) {
37  matches_info.matches.push_back(m0);
38  matches.insert(std::make_pair(m0.queryIdx, m0.trainIdx));
39  }
40  }
41 
42  pair_matches.clear();
43  matcher.knnMatch(descriptors1f, descriptors0f, pair_matches, 2);
44  for (size_t i = 0; i < pair_matches.size(); ++i)
45  {
46  if (pair_matches[i].size() < 2)
47  continue;
48  const cv::DMatch& m0 = pair_matches[i][0];
49  const cv::DMatch& m1 = pair_matches[i][1];
50  if (m0.distance < (1.f - match_conf_) * m1.distance)
51  if (matches.find(std::make_pair(m0.trainIdx, m0.queryIdx)) == matches.end()) // if we haven't already added this pair
52  matches_info.matches.push_back(cv::DMatch(m0.trainIdx, m0.queryIdx, m0.distance));
53  }
54 
55  int num_matches_thresh1_ = 9;
56  // int num_matches_thresh2_ = 9;
57 
58  if (matches_info.matches.size() < static_cast<size_t>(num_matches_thresh1_))
59  return;
60 
61  cv::Mat src_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
62  cv::Mat dst_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
63  std::vector<uint32_t> src_idx, dst_idx;
64 
65  for (size_t i = 0; i < matches_info.matches.size(); ++i) {
66  const cv::DMatch& m = matches_info.matches[i];
67 
68  cv::Point2f p(points0.at<float>(m.queryIdx, 0), points0.at<float>(m.queryIdx, 1));
69  src_points.at<cv::Point2f>(0, static_cast<int>(i)) = p;
70 
71 
72  p = cv::Point2f(points1.at<float>(m.trainIdx, 0), points1.at<float>(m.trainIdx, 1));
73  dst_points.at<cv::Point2f>(0, static_cast<int>(i)) = p;
74  }
75  matches_info.H = cv::findFundamentalMat(src_points, dst_points, cv::RANSAC, 3.0, 0.99, matches_info.inliers_mask);
76  if (matches_info.H.empty())// || std::abs(determinant(matches_info.H)) < std::numeric_limits<double>::epsilon())
77  return;
78 
79 
80  assert(matches_info.matches.size() == matches_info.inliers_mask.size());
81 
82  for (size_t i = 0; i < matches_info.inliers_mask.size(); ++i) {
83  if (matches_info.inliers_mask[i]) {
84  if(inliers0) inliers0->push_back(matches_info.matches[i].queryIdx);
85  if(inliers1) inliers1->push_back(matches_info.matches[i].trainIdx);
86 
87  matches_info.num_inliers++;
88  }
89  }
90 
91  matches_info.confidence = matches_info.num_inliers / (8 + 0.3 * matches_info.matches.size());
92  return;
93  } catch(cv::Exception e) {
94  return;
95  }
96  }
97 
98  void geo_verify_h(const cv::Mat &descriptors0, const cv::Mat &points0,
99  const cv::Mat &descriptors1, const cv::Mat &points1, cv::detail::MatchesInfo &matches_info,
100  std::vector<uint32_t> *inliers0, std::vector<uint32_t> *inliers1) {
101 
102  cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::KDTreeIndexParams>();
103  cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>();
104  matches_info.confidence = 0;
105  matches_info.num_inliers = 0;
106  matches_info.H = cv::Mat::zeros(cv::Size(3, 3), CV_32FC1);
107  if(descriptors1.rows < 9 || descriptors0.rows < 9 || descriptors0.cols < 128 || descriptors1.cols < 128) return;
108  if (descriptors0.depth() == CV_8U) {
109  indexParams->setAlgorithm(cvflann::FLANN_INDEX_LSH);
110  searchParams->setAlgorithm(cvflann::FLANN_INDEX_LSH);
111  }
112  try {
113  cv::FlannBasedMatcher matcher(indexParams, searchParams);
114  std::vector< std::vector<cv::DMatch> > pair_matches;
115  // std::cout << "knn match 1" << std::endl;
116  MatchesSet matches;
117  // Find 1->2 matches
118  matcher.knnMatch(descriptors0, descriptors1, pair_matches, 2);
119  float match_conf_ = 0.2f;
120  for (size_t i = 0; i < pair_matches.size(); ++i) {
121  if (pair_matches[i].size() < 2) continue;
122  const cv::DMatch& m0 = pair_matches[i][0];
123  const cv::DMatch& m1 = pair_matches[i][1];
124  if (m0.distance < (1.f - match_conf_) * m1.distance) {
125  matches_info.matches.push_back(m0);
126  matches.insert(std::make_pair(m0.queryIdx, m0.trainIdx));
127  }
128  }
129 
130  pair_matches.clear();
131  matcher.knnMatch(descriptors1, descriptors0, pair_matches, 2);
132  for (size_t i = 0; i < pair_matches.size(); ++i)
133  {
134  if (pair_matches[i].size() < 2)
135  continue;
136  const cv::DMatch& m0 = pair_matches[i][0];
137  const cv::DMatch& m1 = pair_matches[i][1];
138  if (m0.distance < (1.f - match_conf_) * m1.distance)
139  if (matches.find(std::make_pair(m0.trainIdx, m0.queryIdx)) == matches.end()) // if we haven't already added this pair
140  matches_info.matches.push_back(cv::DMatch(m0.trainIdx, m0.queryIdx, m0.distance));
141  }
142 
143  int num_matches_thresh1_ = 9;
144  // int num_matches_thresh2_ = 9;
145 
146  if (matches_info.matches.size() < static_cast<size_t>(num_matches_thresh1_))
147  return;
148 
149  cv::Mat src_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
150  cv::Mat dst_points(1, static_cast<int>(matches_info.matches.size()), CV_32FC2);
151  std::vector<uint32_t> src_idx, dst_idx;
152 
153  for (size_t i = 0; i < matches_info.matches.size(); ++i) {
154  const cv::DMatch& m = matches_info.matches[i];
155 
156  cv::Point2f p(points0.at<float>(m.queryIdx, 0), points0.at<float>(m.queryIdx, 1));
157  src_points.at<cv::Point2f>(0, static_cast<int>(i)) = p;
158 
159 
160  p = cv::Point2f(points1.at<float>(m.trainIdx, 0), points1.at<float>(m.trainIdx, 1));
161  dst_points.at<cv::Point2f>(0, static_cast<int>(i)) = p;
162  }
163 
164  matches_info.H = cv::findHomography(src_points, dst_points, matches_info.inliers_mask, cv::RANSAC);
165  if (matches_info.H.empty() || std::abs(determinant(matches_info.H)) < std::numeric_limits<double>::epsilon())
166  return;
167 
168 
169  assert(matches_info.matches.size() == matches_info.inliers_mask.size());
170 
171  for (size_t i = 0; i < matches_info.inliers_mask.size(); ++i) {
172  if (matches_info.inliers_mask[i]) {
173  if(inliers0) inliers0->push_back(matches_info.matches[i].queryIdx);
174  if(inliers1) inliers1->push_back(matches_info.matches[i].trainIdx);
175 
176  matches_info.num_inliers++;
177  }
178  }
179 
180  matches_info.confidence = matches_info.num_inliers / (8 + 0.3 * matches_info.matches.size());
181  return;
182  } catch(cv::Exception e) {
183  return;
184  }
185  }
186 
187  bool compute_sparse_sift_feature(const cv::Mat &img, const std::shared_ptr<const SIFTParams> &params ,
188  cv::Mat &keypoints, cv::Mat &descriptors) {
189  std::shared_ptr<const SIFTParams> sift_parameters;
190 
191  if(params == nullptr) {
192  sift_parameters = std::make_shared<const SIFTParams>();
193  }
194 
195  cv::SIFT sift_extractor(sift_parameters->max_features, sift_parameters->num_octave_layers,
196  sift_parameters->contrast_threshold, sift_parameters->edge_threshold, sift_parameters->sigma);
197 
198  cv::Mat descriptors_f;
199 
200  if(img.size().area() > 0) {
201 
202  std::vector<cv::KeyPoint> keypoint_vec;
203  sift_extractor.detect(img, keypoint_vec);
204  sift_extractor.compute(img, keypoint_vec, descriptors_f);
205  descriptors_f.convertTo(descriptors, CV_8UC1);
206  keypoints = cv::Mat(keypoint_vec.size(), 2, CV_32FC1);
207 
208  for(int i=0; i<(int)keypoint_vec.size(); i++) {
209  keypoints.at<float>(i, 0) = keypoint_vec[i].pt.x;
210  keypoints.at<float>(i, 1) = keypoint_vec[i].pt.y;
211  }
212 
213  return true;
214  }
215 
216 
217 
218  return false;
219  }
220 
221  bool compute_bow_feature(const cv::Mat& descriptors, const cv::Ptr<cv::DescriptorMatcher> &matcher,
222  cv::Mat& bow_descriptors, std::shared_ptr< std::vector<std::vector<uint32_t> > > cluster_indices) {
223 
224  int clusterCount = matcher->getTrainDescriptors()[0].rows;
225 
226  std::vector<cv::DMatch> matches;
227  matcher->match(descriptors, matches);
228 
229  if(cluster_indices != nullptr) {
230  cluster_indices->clear();
231  cluster_indices->resize(clusterCount);
232  }
233 
234  bow_descriptors.release();
235  bow_descriptors = cv::Mat( 1, clusterCount, CV_32FC1, cv::Scalar::all(0.0) );
236  float *dptr = (float*)bow_descriptors.data;
237  for(size_t i=0; i < matches.size(); i++) {
238  int queryIdx = matches[i].queryIdx;
239  int trainIdx = matches[i].trainIdx;
240 
241  dptr[trainIdx] = dptr[trainIdx] + 1.f;
242  if(cluster_indices != nullptr) {
243  (*cluster_indices)[trainIdx].push_back( queryIdx );
244  }
245  }
246 
247  return true;
248  }
249 
250  cv::Ptr<cv::DescriptorMatcher> construct_descriptor_matcher(const cv::Mat &vocabulary) {
251  cv::Ptr<cv::DescriptorMatcher> matcher = cv::makePtr<cv::FlannBasedMatcher>();
252  matcher->add(std::vector<cv::Mat>(1, vocabulary));
253  return matcher;
254  }
255 
256  cv::Mat merge_descriptors(std::vector<cv::Mat> &descriptors, bool release_original) {
257  uint64_t descriptor_count = 0;
258  for (size_t i = 0; i < descriptors.size(); i++) {
259  descriptor_count += descriptors[i].rows;
260  }
261 
262  cv::Mat merged(descriptor_count, descriptors[0].cols, descriptors[0].type());
263  for (size_t i = 0, start = 0; i < descriptors.size(); i++) {
264  cv::Mat submut = merged.rowRange((int)start, (int)(start + descriptors[i].rows));
265  descriptors[i].copyTo(submut);
266  if (release_original) descriptors[i].release();
267  start += descriptors[i].rows;
268  }
269 
270  return merged;
271  }
272 
273  bool is_good_match(const cv::detail::MatchesInfo &matches_info) {
274  return matches_info.num_inliers >= 16 && matches_info.confidence > 0.7;
275  }
276 }