Hugintrunk  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
RansacFiltering.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2007-2008 Anael Orlinski
3 *
4 * This file is part of Panomatic.
5 *
6 * Panomatic is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * Panomatic is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with Panomatic; if not, write to the Free Software
18 * <http://www.gnu.org/licenses/>.
19 */
20 
21 #include "RansacFiltering.h"
22 #include "Homography.h"
23 
24 static int genint(int x)
25 {
26  return (int)((double)rand()*x/(double)RAND_MAX);
27 }
28 
29 namespace lfeat
30 {
31 
32 // distance between estimate and real point in pixels.
33 
35 {
36  double x1p, y1p;
37  aH->transformPoint(aM._img1_x, aM._img1_y, x1p, y1p);
38 
39  double d1 = aM._img2_x - x1p;
40  double d2 = aM._img2_y - y1p;
41 
42  return d1*d1+d2*d2;
43 }
44 
45 
46 void Ransac::filter(PointMatchVector_t& ioMatches, PointMatchVector_t& ioRemovedMatches)
47 {
48  int aRemainingIterations = _nIter;
49  const double aErrorDistSq = _distanceThres * _distanceThres;
50 
51  Homography aCurrentModel;
52 
53  unsigned int aMaxInliers = 0;
54  PointMatchVector_t aBestInliers;
55  PointMatchVector_t aBestOutliers;
56 
57  // normalization !!!!!!
58  aCurrentModel.initMatchesNormalization(ioMatches);
59 
60 
61  //std::cout << "gravity " << _v1x << " " << _v1y << " " << _v2x << " " << _v2y << endl;
62 
63  for(; aRemainingIterations > 0; aRemainingIterations--)
64  {
65  //cout << "RANSAC -- iter " << aRemainingIterations << endl;
66 
67  // random select 4 matches to fit the model
68  // from the input set as maybe_inliers
69  PointMatchVector_t aMatches(ioMatches);
70  PointMatchVector_t aInliers, aOutliers;
71 
72  //std::cout << aMatches.size() << " matches size" << endl;
73 
74  for(int i=0; i<5; i++)
75  {
76  int n = genint((int)aMatches.size()-1);
77  aInliers.push_back(aMatches.at(n));
78  aMatches.erase(aMatches.begin()+n);
79  }
80 
81  //std::cout << aMatches.size() << " estimate" << endl;
82 
83  if (!aCurrentModel.estimate(aInliers))
84  {
85  aMatches.clear();
86  continue;
87  }
88 
89  // for every point remaining in aMatches, add them to aInliers if they fit the model well.
90  for (size_t i = 0; i < aMatches.size(); ++i)
91  {
92  PointMatchPtr aMatchesIter2 = aMatches[i];
93  if (calcError(&aCurrentModel, *aMatchesIter2) < aErrorDistSq)
94  {
95  aInliers.push_back(aMatchesIter2);
96  }
97  else
98  {
99  aOutliers.push_back(aMatchesIter2);
100  }
101  }
102 
103  if (aInliers.size() > aMaxInliers)
104  {
105  //cout << "good found -----------------" << aRemainingIterations << endl;
106  //cout << aCurrentModel << endl;
107  for (int i=0; i<3; ++i)
108  for(int j=0; j<3; ++j)
109  {
110  _bestModel._H[i][j] = aCurrentModel._H[i][j];
111  }
112 
113  _bestModel._v1x = aCurrentModel._v1x;
114  _bestModel._v2x = aCurrentModel._v2x;
115  _bestModel._v1y = aCurrentModel._v1y;
116  _bestModel._v2y = aCurrentModel._v2y;
117 
118 
119  //*ioBestModel = *aCurrentModel;
120  aMaxInliers = (unsigned int)aInliers.size();
121  aBestInliers = aInliers;
122  aBestOutliers = aOutliers;
123  //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
124 
125  }
126 
127  // if there are 0 outliers then we are done.
128  if (aOutliers.empty())
129  {
130  break;
131  }
132 
133  //cout << "Inliers : " << aInliers.size() << " Outliers : " << aOutliers.size() << endl;
134  //cout << "end iter" << endl;
135  }
136 
137  ioMatches = aBestInliers;
138  ioRemovedMatches = aBestOutliers;
139 
140 
141 }
142 
143 void Ransac::transform(double iX, double iY, double& oX, double& oY)
144 {
145  _bestModel.transformPoint(iX, iY, oX, oY);
146 }
147 
148 }
static int genint(int x)
std::vector< PointMatchPtr > PointMatchVector_t
Definition: PointMatch.h:53
void initMatchesNormalization(PointMatchVector_t &iMatches)
Definition: Homography.cpp:100
void transformPoint(double iX, double iY, double &oX, double &oY)
Definition: Homography.cpp:172
void transform(double iX, double iY, double &oX, double &oY)
Homography _bestModel
void filter(std::vector< PointMatchPtr > &ioMatches, std::vector< PointMatchPtr > &ioRemovedMatches)
double calcError(Homography *aH, PointMatch &aM)
bool estimate(PointMatchVector_t &iMatches)
Definition: Homography.cpp:182
std::shared_ptr< PointMatch > PointMatchPtr
Definition: PointMatch.h:52
double _H[3][3]
Definition: Homography.h:65