IVT
RANSAC.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: RANSAC.cpp
37 // Author: Pedram Azad
38 // Date: 14.12.2009
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "RANSAC.h"
49 
50 #include "Structs/Structs.h"
51 #include "Math/Math2d.h"
52 #include "Math/Math3d.h"
53 #include "Math/LinearAlgebra.h"
54 
55 #include <stdlib.h>
56 #include <math.h>
57 
58 
59 
60 // ****************************************************************************
61 // Functions
62 // ****************************************************************************
63 
64 bool RANSAC::RANSACAffineTransformation(const CDynamicArrayTemplate<PointPair2d> &matchCandidates, CDynamicArrayTemplate<PointPair2d> &resultMatches, float fRANSACThreshold, int nIterations)
65 {
66  const int nMatchCandidates = matchCandidates.GetSize();
67 
68  if (nMatchCandidates < 3)
69  {
70  printf("error: at least 3 match candidates must be provided for RANSAC::RANSACAffineTransformation (%i provided)\n", nMatchCandidates);
71  return false;
72  }
73 
74  Mat3d best_B;
75  int i, max = 0;
76 
77  for (i = 0; i < nIterations; i++)
78  {
79  // identify 3 different points
80  const int nFirstIndex = rand() % nMatchCandidates;
81 
82  int nTempIndex;
83 
84  do { nTempIndex = rand() % nMatchCandidates; } while (nTempIndex == nFirstIndex);
85  const int nSecondIndex = nTempIndex;
86 
87  do { nTempIndex = rand() % nMatchCandidates; } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex);
88 
89  Vec2d pFeaturesLeft[3];
90  Vec2d pFeaturesRight[3];
91 
92  Math2d::SetVec(pFeaturesLeft[0], matchCandidates[nFirstIndex].p1);
93  Math2d::SetVec(pFeaturesLeft[1], matchCandidates[nSecondIndex].p1);
94  Math2d::SetVec(pFeaturesLeft[2], matchCandidates[nTempIndex].p1);
95 
96  Math2d::SetVec(pFeaturesRight[0], matchCandidates[nFirstIndex].p2);
97  Math2d::SetVec(pFeaturesRight[1], matchCandidates[nSecondIndex].p2);
98  Math2d::SetVec(pFeaturesRight[2], matchCandidates[nTempIndex].p2);
99 
100  // calculate affine transformation for these points
101  Mat3d B;
102  LinearAlgebra::DetermineAffineTransformation(pFeaturesRight, pFeaturesLeft, 3, B);
103 
104  // count support
105  int nSupport = 0;
106  for (int j = 0; j < nMatchCandidates; j++)
107  {
108  Vec2d p;
109  Math2d::ApplyHomography(B, matchCandidates[j].p2, p);
110 
111  const float distance = Math2d::Distance(p, matchCandidates[j].p1);
112 
113  if (distance < fRANSACThreshold)
114  nSupport++;
115  }
116 
117  // store if it is the current maximum
118  if (nSupport > max)
119  {
120  max = nSupport;
121  Math3d::SetMat(best_B, B);
122  }
123  }
124 
125  // filter matches
126  resultMatches.Clear();
127 
128  for (i = 0; i < nMatchCandidates; i++)
129  {
130  Vec2d p;
131  Math2d::ApplyHomography(best_B, matchCandidates[i].p2, p);
132 
133  const float distance = Math2d::Distance(p, matchCandidates[i].p1);
134 
135  if (distance < fRANSACThreshold)
136  {
137  PointPair2d match;
138  Math2d::SetVec(match.p1, matchCandidates[i].p1);
139  Math2d::SetVec(match.p2, matchCandidates[i].p2);
140 
141  resultMatches.AddElement(match);
142  }
143  }
144 
145  return true;
146 }
147 
148 bool RANSAC::RANSACHomography(const CDynamicArrayTemplate<PointPair2d> &matchCandidates, CDynamicArrayTemplate<PointPair2d> &resultMatches, float fRANSACThreshold, int nIterations)
149 {
150  const int nMatchCandidates = matchCandidates.GetSize();
151 
152  if (nMatchCandidates < 4)
153  {
154  printf("error: at least 4 match candidates must be provided for RANSAC::RANSACHomography (%i provided)\n", nMatchCandidates);
155  return false;
156  }
157 
158  Mat3d best_B;
159  int i, max = 0;
160 
161  for (i = 0; i < nIterations; i++)
162  {
163  // identify 4 different points
164  const int nFirstIndex = rand() % nMatchCandidates;
165 
166  int nTempIndex;
167 
168  do { nTempIndex = rand() % nMatchCandidates; } while (nTempIndex == nFirstIndex);
169  const int nSecondIndex = nTempIndex;
170 
171  do { nTempIndex = rand() % nMatchCandidates; } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex);
172  const int nThirdIndex = nTempIndex;
173 
174  do { nTempIndex = rand() % nMatchCandidates; } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex || nTempIndex == nThirdIndex);
175 
176  Vec2d pFeaturesLeft[4];
177  Vec2d pFeaturesRight[4];
178 
179  Math2d::SetVec(pFeaturesLeft[0], matchCandidates[nFirstIndex].p1);
180  Math2d::SetVec(pFeaturesLeft[1], matchCandidates[nSecondIndex].p1);
181  Math2d::SetVec(pFeaturesLeft[2], matchCandidates[nThirdIndex].p1);
182  Math2d::SetVec(pFeaturesLeft[3], matchCandidates[nTempIndex].p1);
183 
184  Math2d::SetVec(pFeaturesRight[0], matchCandidates[nFirstIndex].p2);
185  Math2d::SetVec(pFeaturesRight[1], matchCandidates[nSecondIndex].p2);
186  Math2d::SetVec(pFeaturesRight[2], matchCandidates[nThirdIndex].p2);
187  Math2d::SetVec(pFeaturesRight[3], matchCandidates[nTempIndex].p1);
188 
189  // calculate affine transformation for these points
190  Mat3d B;
191  LinearAlgebra::DetermineHomography(pFeaturesRight, pFeaturesLeft, 4, B, false);
192 
193  // count support
194  int nSupport = 0;
195  for (int j = 0; j < nMatchCandidates; j++)
196  {
197  Vec2d p;
198  Math2d::ApplyHomography(B, matchCandidates[j].p2, p);
199 
200  const float distance = Math2d::Distance(p, matchCandidates[j].p1);
201 
202  if (distance < fRANSACThreshold)
203  nSupport++;
204  }
205 
206  // store if it is the current maximum
207  if (nSupport > max)
208  {
209  max = nSupport;
210  Math3d::SetMat(best_B, B);
211  }
212  }
213 
214  // filter matches
215  resultMatches.Clear();
216 
217  for (i = 0; i < nMatchCandidates; i++)
218  {
219  Vec2d p;
220  Math2d::ApplyHomography(best_B, matchCandidates[i].p2, p);
221 
222  const float distance = Math2d::Distance(p, matchCandidates[i].p1);
223 
224  if (distance < fRANSACThreshold)
225  {
226  PointPair2d match;
227  Math2d::SetVec(match.p1, matchCandidates[i].p1);
228  Math2d::SetVec(match.p2, matchCandidates[i].p2);
229 
230  resultMatches.AddElement(match);
231  }
232  }
233 
234  return true;
235 }
236 
237 bool RANSAC::RANSAC3DPlane(const CVec3dArray &pointCandidates, CVec3dArray &resultPoints, float fRANSACThreshold, int nIterations)
238 {
239  const int nPointCandidates = pointCandidates.GetSize();
240 
241  if (nPointCandidates < 3)
242  {
243  printf("error: at least 3 point candidates must be provided for RANSAC::RANSAC3DPlane (%i provided)\n", nPointCandidates);
244  return false;
245  }
246 
247  int i, max = 0;
248  float best_c = 0.0f;
249  Vec3d best_n = { 0.0f, 0.0f };
250 
251  for (i = 0; i < nIterations; i++)
252  {
253  // identify 3 different points
254  const int nFirstIndex = rand() % nPointCandidates;
255 
256  int nTempIndex;
257 
258  do { nTempIndex = rand() % nPointCandidates; } while (nTempIndex == nFirstIndex);
259  const int nSecondIndex = nTempIndex;
260 
261  do { nTempIndex = rand() % nPointCandidates; } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex);
262 
263  const Vec3d &p1 = pointCandidates[nFirstIndex];
264  const Vec3d &p2 = pointCandidates[nSecondIndex];
265  const Vec3d &p3 = pointCandidates[nTempIndex];
266 
267  Vec3d u1, u2, n;
268  Math3d::SubtractVecVec(p2, p1, u1);
269  Math3d::SubtractVecVec(p3, p1, u2);
270  Math3d::CrossProduct(u1, u2, n);
272  const float c = Math3d::ScalarProduct(n, p1);
273 
274  // count support
275  int nSupport = 0;
276  for (int j = 0; j < nPointCandidates; j++)
277  if (fabsf(Math3d::ScalarProduct(n, pointCandidates[j]) - c) <= fRANSACThreshold)
278  nSupport++;
279 
280  // store if it is the current maximum
281  if (nSupport > max)
282  {
283  max = nSupport;
284  Math3d::SetVec(best_n, n);
285  best_c = c;
286  }
287  }
288 
289  // filter points
290  resultPoints.Clear();
291 
292  for (i = 0; i < nPointCandidates; i++)
293  {
294  if (fabsf(Math3d::ScalarProduct(pointCandidates[i], best_n) - best_c) <= fRANSACThreshold)
295  resultPoints.AddElement(pointCandidates[i]);
296  }
297 
298  return true;
299 }
bool RANSACHomography(const CDynamicArrayTemplate< PointPair2d > &matchCandidates, CDynamicArrayTemplate< PointPair2d > &resultMatches, float fRANSACThreshold=5.0f, int nIterations=500)
Definition: RANSAC.cpp:148
void ApplyHomography(const Mat3d &A, const Vec2d &p, Vec2d &result)
Definition: Math2d.cpp:253
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
float ScalarProduct(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:559
bool DetermineAffineTransformation(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD=false)
Determines an affine transformation based on a set of 2d-2d point correspondences.
void NormalizeVec(Vec3d &vec)
Definition: Math3d.cpp:573
void SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:522
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
bool DetermineHomography(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD=false)
Determines a homography based on a set of 2d-2d point correspondences.
Vec2d p1
Definition: Structs.h:73
bool RANSACAffineTransformation(const CDynamicArrayTemplate< PointPair2d > &matchCandidates, CDynamicArrayTemplate< PointPair2d > &resultMatches, float fRANSACThreshold=5.0f, int nIterations=500)
Definition: RANSAC.cpp:64
bool RANSAC3DPlane(const CVec3dArray &pointCandidates, CVec3dArray &resultPoints, float fRANSACThreshold=5.0f, int nIterations=500)
Definition: RANSAC.cpp:237
float Distance(const Vec2d &vector1, const Vec2d &vector2)
Definition: Math2d.cpp:181
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
void SetVec(Vec2d &vec, float x, float y)
Definition: Math2d.cpp:68
void AddElement(const T &element)
void SetVec(Vec3d &vec, float x, float y, float z)
Definition: Math3d.cpp:243
Vec2d p2
Definition: Structs.h:74
void CrossProduct(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:564