image_matching.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
31 
32 #include <algorithm>
33 #include <vector>
34 
35 #include <opencv2/imgproc/imgproc.hpp>
36 #include <opencv2/highgui/highgui.hpp>
37 #include <opencv2/calib3d/calib3d.hpp>
38 
39 #include <ros/ros.h>
40 
41 namespace swri_image_util
42 {
43  void GetFundamentalInliers(const cv::Mat points1,
44  const cv::Mat points2,
45  cv::Mat& fundamental_matrix,
46  cv::Mat& inliers1,
47  cv::Mat& inliers2,
48  double max_distance,
49  double confidence)
50  {
51  std::vector<uint32_t> indices;
53  points1, points2,
54  fundamental_matrix,
55  inliers1, inliers2,
56  indices,
57  max_distance,
58  confidence);
59  }
60 
61  void GetFundamentalInliers(const cv::Mat points1,
62  const cv::Mat points2,
63  cv::Mat& fundamental_matrix,
64  cv::Mat& inliers1,
65  cv::Mat& inliers2,
66  std::vector<uint32_t>& indices,
67  double max_distance,
68  double confidence)
69  {
70  std::vector<uchar> status;
71  fundamental_matrix = cv::findFundamentalMat(
72  points1,
73  points2,
74  status,
75  cv::FM_RANSAC,
76  max_distance,
77  confidence);
78 
79  int inliers = 0;
80  for (unsigned int i = 0; i < status.size(); i++)
81  {
82  if (status[i])
83  {
84  inliers++;
85  }
86  }
87 
88  indices.resize(inliers);
89 
90  if (inliers > 0)
91  {
92  inliers1 = cv::Mat(cv::Size(1, inliers), CV_32FC2);
93  inliers2 = cv::Mat(cv::Size(1, inliers), CV_32FC2);
94 
95  int index = 0;
96  for (unsigned int i = 0; i < status.size(); i++)
97  {
98  if (status[i])
99  {
100  inliers1.at<cv::Vec2f>(0, index) = points1.at<cv::Vec2f>(0, i);
101  inliers2.at<cv::Vec2f>(0, index) = points2.at<cv::Vec2f>(0, i);
102  indices[index] = i;
103  index++;
104  }
105  }
106  }
107  }
108 
109  void ConvertMatches(const std::vector<cv::KeyPoint>& kp1,
110  const std::vector<cv::KeyPoint>& kp2,
111  const std::vector<cv::DMatch>& matches,
112  cv::Mat& kp1_out,
113  cv::Mat& kp2_out)
114  {
115  kp1_out.release();
116  kp2_out.release();
117  kp1_out.create(cv::Size(1, matches.size()), CV_32FC2);
118  kp2_out.create(cv::Size(1, matches.size()), CV_32FC2);
119 
120  for (unsigned int i = 0; i < matches.size(); i++)
121  {
122  kp1_out.at<cv::Vec2f>(0, i) = kp1[matches[i].queryIdx].pt;
123  kp2_out.at<cv::Vec2f>(0, i) = kp2[matches[i].trainIdx].pt;
124  }
125  }
126 }
void GetFundamentalInliers(const cv::Mat points1, const cv::Mat points2, cv::Mat &fundamental_matrix, cv::Mat &inliers1, cv::Mat &inliers2, double max_distance=1.0, double confidence=0.99)
Computes the fundamental matrix for a set of matching points in two different images. The method also returns the inlier keypoints for both frames.
void ConvertMatches(const std::vector< cv::KeyPoint > &kp1, const std::vector< cv::KeyPoint > &kp2, const std::vector< cv::DMatch > &matches, cv::Mat &kp1_out, cv::Mat &kp2_out)
Converts keypoints and matches into two cv::Mats in which the the matching keypoints from kp1 and kp2...


swri_image_util
Author(s): Kris Kozak
autogenerated on Tue Apr 6 2021 02:50:39