image_matching.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <swri_image_util/image_matching.h>
00031 
00032 #include <algorithm>
00033 #include <vector>
00034 
00035 #include <opencv2/imgproc/imgproc.hpp>
00036 #include <opencv2/highgui/highgui.hpp>
00037 #include <opencv2/calib3d/calib3d.hpp>
00038 
00039 #include <ros/ros.h>
00040 
00041 namespace swri_image_util
00042 {
00043   void GetFundamentalInliers(const cv::Mat points1,
00044                              const cv::Mat points2,
00045                              cv::Mat& fundamental_matrix,
00046                              cv::Mat& inliers1,
00047                              cv::Mat& inliers2,
00048                              double max_distance,
00049                              double confidence)
00050   {
00051     std::vector<uint32_t> indices;
00052     GetFundamentalInliers(
00053       points1, points2,
00054       fundamental_matrix,
00055       inliers1, inliers2,
00056       indices,
00057       max_distance,
00058       confidence);
00059   }
00060 
00061   void GetFundamentalInliers(const cv::Mat points1,
00062                              const cv::Mat points2,
00063                              cv::Mat& fundamental_matrix,
00064                              cv::Mat& inliers1,
00065                              cv::Mat& inliers2,
00066                              std::vector<uint32_t>& indices,
00067                              double max_distance,
00068                              double confidence)
00069   {
00070     std::vector<uchar> status;
00071     fundamental_matrix = cv::findFundamentalMat(
00072       points1,
00073       points2,
00074       status,
00075       CV_FM_RANSAC,
00076       max_distance,
00077       confidence);
00078 
00079     int inliers = 0;
00080     for (unsigned int i = 0; i < status.size(); i++)
00081     {
00082       if (status[i])
00083       {
00084         inliers++;
00085       }
00086     }
00087 
00088     indices.resize(inliers);
00089 
00090     if (inliers > 0)
00091     {
00092       inliers1 = cv::Mat(cv::Size(1, inliers), CV_32FC2);
00093       inliers2 = cv::Mat(cv::Size(1, inliers), CV_32FC2);
00094 
00095       int index = 0;
00096       for (unsigned int i = 0; i < status.size(); i++)
00097       {
00098         if (status[i])
00099         {
00100           inliers1.at<cv::Vec2f>(0, index) = points1.at<cv::Vec2f>(0, i);
00101           inliers2.at<cv::Vec2f>(0, index) = points2.at<cv::Vec2f>(0, i);
00102           indices[index] = i;
00103           index++;
00104         }
00105       }
00106     }
00107   }
00108 
00109   void ConvertMatches(const std::vector<cv::KeyPoint>& kp1,
00110                       const std::vector<cv::KeyPoint>& kp2,
00111                       const std::vector<cv::DMatch>& matches,
00112                       cv::Mat& kp1_out,
00113                       cv::Mat& kp2_out)
00114   {
00115     kp1_out.release();
00116     kp2_out.release();
00117     kp1_out.create(cv::Size(1, matches.size()), CV_32FC2);
00118     kp2_out.create(cv::Size(1, matches.size()), CV_32FC2);
00119 
00120     for (unsigned int i = 0; i < matches.size(); i++)
00121     {
00122       kp1_out.at<cv::Vec2f>(0, i) = kp1[matches[i].queryIdx].pt;
00123       kp2_out.at<cv::Vec2f>(0, i) = kp2[matches[i].trainIdx].pt;
00124     }
00125   }
00126 }


swri_image_util
Author(s): Kris Kozak
autogenerated on Tue Oct 3 2017 03:19:34