CvFundamentalMat.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002  *  CvFundamentalMat.cpp
00003  *
00004  *  (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
00005  *           Universitaet Koblenz-Landau
00006  *******************************************************************************/
00007 
00008 #include "CvFundamentalMat.h"
00009 
00010 #include "Architecture/Config/Config.h"
00011 
00012 #include <math.h>
00013 
00014 #define THIS CvFundamentalMat
00015 
00016 using namespace std;
00017 
00018 THIS::THIS ( vector< KeyPoint >* keyPoints1, vector< KeyPoint >* keyPoints2, std::list< KeyPointMatch >& matches )
00019 {
00020   m_KeyPoints1 = keyPoints1;
00021   m_KeyPoints2 = keyPoints2;
00022   m_Matches = matches;
00023   m_Success = false;
00024   m_MaxReprojectionError = Config::getFloat( "ObjectRecognition.Ransac.fMaxReprojectionError" );
00025 }
00026 
00027 THIS::~THIS()
00028 {
00029 }
00030 
00031 bool THIS::computeFundamentalMat()
00032 {
00033   double fundamentalMat[9];
00034 
00035   memset ( fundamentalMat, 0, 9*sizeof ( double ) );
00036   m_FundMatCv = cvMat ( 3, 3, CV_64F, fundamentalMat );
00037 
00038   std::vector<CvPoint2D32f> points1Cv, points2Cv;
00039 
00040 
00041   int numMatches = m_Matches.size();
00042 
00043   if ( numMatches < 4 )
00044   {
00045     return false;
00046   }
00047 
00048   // Set vectors to correct size
00049   points1Cv.resize ( numMatches );
00050   points2Cv.resize ( numMatches );
00051 
00052   // Copy Ipoints from match vector into cvPoint vectors
00053   std::list<KeyPointMatch>::iterator currentMatch = m_Matches.begin();
00054   int i = 0;
00055   while ( currentMatch != m_Matches.end() )
00056   {
00057     points1Cv[i] = cvPoint2D32f ( ( *m_KeyPoints1 ) [ currentMatch->index1 ].x,
00058                                   ( *m_KeyPoints1 ) [ currentMatch->index1 ].y );
00059     points2Cv[i] = cvPoint2D32f ( ( *m_KeyPoints2 ) [ currentMatch->index2 ].x,
00060                                   ( *m_KeyPoints2 ) [ currentMatch->index2 ].y );
00061     currentMatch++;
00062     i++;
00063   }
00064 
00065   m_Points1CvMat = cvMat ( 1, numMatches, CV_32FC2, &points1Cv[0] );
00066   m_Points2CvMat = cvMat ( 1, numMatches, CV_32FC2, &points2Cv[0] );
00067 
00068   int method = 0;
00069   switch (Config::getInstance()->getInt( "ObjectRecognition.FundamentalMat.iMethod" ))
00070   {
00071     case 0 :
00072       method = CV_FM_7POINT;
00073       break;
00074     case 1 :
00075       method = CV_FM_8POINT;
00076       break;
00077     case 2 :
00078       method = CV_FM_RANSAC;
00079       break;
00080     case 3:
00081       method = CV_FM_LMEDS;
00082       break;
00083     default:
00084       ROS_ERROR_STREAM("Undefined methode to find fundamental matrix");
00085       break;
00086   }
00087   CvMat* status;
00088   status = cvCreateMat(1,numMatches,CV_8UC1);
00089 
00090 
00091 //int cvFindFundamentalMat(const CvMat* points1, const CvMat* points2, CvMat* fundamental_matrix, int method=CV_FM_RANSAC, double param1=1., double param2=0.99, CvMat* status=NULL)
00092   //returns the number of fundamental matrices found (1 or 3) and 0, if no matrix is found.
00093   m_Success = cvFindFundamentalMat( &m_Points2CvMat, &m_Points1CvMat, &m_FundMatCv, method, m_MaxReprojectionError,0.99,status);
00094 
00095   ROS_INFO_STREAM("Status = " << m_Success );
00096 
00097   return m_Success > 0 ? true : false;
00098 }
00099 
00100 void THIS::eliminateBadMatches()
00101 {
00102   vector<Point2D> points2;
00103   vector<Point2D> projPoints;
00104 
00105   points2.reserve( m_Matches.size() );
00106 
00107   std::list<KeyPointMatch>::iterator currentMatch = m_Matches.begin();
00108   while ( currentMatch != m_Matches.end() )
00109   {
00110     Point2D pos2 = ( *m_KeyPoints2 ) [ currentMatch->index2 ].position();
00111     points2.push_back( pos2 );
00112     currentMatch++;
00113   }
00114 
00115 //  void cvComputeCorrespondEpilines(const CvMat* points, int which_image, const CvMat* fundamental_matrix, CvMat* correspondent_lines)
00116 //which image?
00117   //Tranform object points to scene by fundamental mat
00118 
00119   double correspondent_lines[3];
00120 
00121   memset ( correspondent_lines, 0, 3*sizeof ( double ) );
00122 
00123   CvMat correspondent_linesCV;
00124   correspondent_linesCV= cvMat(3,m_Matches.size(),CV_32F,correspondent_lines);
00125 
00126   cvComputeCorrespondEpilines(&m_Points2CvMat, 1, &m_FundMatCv, &correspondent_linesCV);
00127 
00128   //Compare line distance to point distance
00129 
00130   currentMatch = m_Matches.begin();
00131   int i = 0;
00132   while ( currentMatch != m_Matches.end() )
00133   {
00134 //    Point2D pos1 = ( *m_KeyPoints1 ) [ currentMatch->index1 ].position();
00135 //    float scale = ( *m_KeyPoints1 ) [ currentMatch->index1 ].scale;
00136 //    if ( pos1.distance( projPoints[i] ) > m_MaxReprojectionError*scale )
00137 //     int x = correspondent_lines[i].x;
00138 //     int y = correspondent_lines[i].y;
00139 //    if()
00140 //    {
00141 //      currentMatch = m_Matches.erase( currentMatch );
00142 //    }
00143 //    else
00144 //    {
00145 //      currentMatch++;
00146 //    }
00147     i++;
00148   }
00149 }
00150 
00151 
00152 #undef THIS


or_libs
Author(s): Viktor Seib
autogenerated on Tue Jan 7 2014 11:24:03