Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
CornerMatcher.cpp
1 /*
2 This file is part of the BIAS library (Basic ImageAlgorithmS).
3 
4 Copyright (C) 2003-2009 (see file CONTACT for details)
5  Multimediale Systeme der Informationsverarbeitung
6  Institut fuer Informatik
7  Christian-Albrechts-Universitaet Kiel
8 
9 
10 BIAS is free software; you can redistribute it and/or modify
11 it under the terms of the GNU Lesser General Public License as published by
12 the Free Software Foundation; either version 2.1 of the License, or
13 (at your option) any later version.
14 
15 BIAS is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU Lesser General Public License for more details.
19 
20 You should have received a copy of the GNU Lesser General Public License
21 along with BIAS; if not, write to the Free Software
22 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 */
24 
25 
26 #include "CornerMatcher.hh"
27 
28 using namespace BIAS;
29 using namespace std;
30 
31 
32 #define INST(uctype)\
33 template BIASMatcher2D_EXPORT int CornerMatcher::SSD<uctype>(const HomgPoint2D& p1, const HomgPoint2D& p2,\
34  const Image<uctype>& im1,\
35  const Image<uctype>& im2, const unsigned int halfwinsize, double& result)const ;\
36 template BIASMatcher2D_EXPORT int CornerMatcher::NCC<uctype>(const HomgPoint2D& p1, const HomgPoint2D& p2,\
37  const Image<uctype>& im1,\
38  const Image<uctype>& im2, const unsigned int halfwinsize, double& result)const ;\
39 template BIASMatcher2D_EXPORT int CornerMatcher::NCCSearch<uctype>(const HomgPoint2D& p1, const HomgPoint2D& p2,\
40  const Image<uctype>& im1, const Image<uctype>& im2, \
41  const unsigned int halfnccwinsize, const unsigned int halfsearchwinsize, \
42  HomgPoint2D& resultpoint,\
43  const double mincorrelation, \
44  double& result);\
45 template BIASMatcher2D_EXPORT int CornerMatcher::ParabolaNCC(const HomgPoint2D& p1, const HomgPoint2D& p2,\
46  const Image<uctype>& im1, const Image<uctype>& im2, const unsigned int halfwinsize,\
47  HomgPoint2D& result)const ;\
48 template BIASMatcher2D_EXPORT int CornerMatcher::ParabolaNCC5(const HomgPoint2D& p1, const HomgPoint2D& p2,\
49  const Image<uctype>& im1, const Image<uctype>& im2, const unsigned int halfwinsize,\
50  HomgPoint2D& result)const ;\
51 template BIASMatcher2D_EXPORT int CornerMatcher::RefinePyramideNCC<uctype>(\
52  std::vector<std::vector<HomgPoint2D> >& ps1, PyramidImage<uctype>& im1, \
53  PyramidImage<uctype>& im2, unsigned int halfnccwinsize, \
54  std::vector<std::vector<HomgPoint2D> >& matches, \
55  std::vector<std::vector<double> >& matchquality)
56 
57 
58 
59 INST(unsigned char);
60 INST(float);
61 
62 // fill in instances as required
63 #ifdef BUILD_IMAGE_INT
64 INST(int);
65 #endif
66 #ifdef BUILD_IMAGE_CHAR
67 INST(char);
68 #endif
69 #ifdef BUILD_IMAGE_SHORT
70 INST(short);
71 #endif
72 #ifdef BUILD_IMAGE_USHORT
73 INST(unsigned short);
74 #endif
75 #ifdef BUILD_IMAGE_UINT
76 INST(unsigned int);
77 #endif
78 #ifdef BUILD_IMAGE_DOUBLE
79 INST(double);
80 #endif
81 
82 
83 CornerMatcher::CornerMatcher()
84  : RegionMatcher()
85 {}
86 
87 CornerMatcher::~CornerMatcher()
88 {}
89 
90 template <class StorageType>
91 int CornerMatcher::
92 SSD(const HomgPoint2D& p1, const HomgPoint2D& p2,
93  const Image<StorageType>& im1, const Image<StorageType>& im2,
94  const unsigned int halfwinsize, double& result) const
95 {
96  int res=0;
97  int ip1[2], ip2[2];
98  unsigned int uip1[2], uip2[2];
99  if (p1.IsAtInfinity() || p2.IsAtInfinity())
100  res=-2;
101  else {
102  ip1[0]=(int)rint(p1[0]/p1[2]);
103  ip1[1]=(int)rint(p1[1]/p1[2]);
104  ip2[0]=(int)rint(p2[0]/p2[2]);
105  ip2[1]=(int)rint(p2[1]/p2[2]);
106  if (ip1[0]<(int)halfwinsize ||
107  ip1[0]+(int)halfwinsize>=(int)im1.GetWidth() ||
108  ip1[1]<(int)halfwinsize ||
109  ip1[1]+(int)halfwinsize>=(int)im1.GetHeight() ||
110  ip2[0]<(int)halfwinsize ||
111  ip2[0]+(int)halfwinsize>=(int)im2.GetWidth() ||
112  ip2[1]<(int)halfwinsize ||
113  ip2[1]+(int)halfwinsize>=(int)im2.GetHeight())
114  res=-1;
115  else{
116  uip1[0]=(unsigned int)ip1[0];
117  uip1[1]=(unsigned int)ip1[1];
118  uip2[0]=(unsigned int)ip2[0];
119  uip2[1]=(unsigned int)ip2[1];
120  RegionMatcher::SSD(uip1, uip2, im1.GetImageDataArray(),
121  im2.GetImageDataArray(), halfwinsize, result);
122  }
123  }
124  return res;
125 }
126 
127 template <class StorageType>
128 int CornerMatcher::
129 NCC(const HomgPoint2D& p1, const HomgPoint2D& p2,
130  const Image<StorageType>& im1, const Image<StorageType>& im2,
131  const unsigned int halfwinsize, double& result) const
132 {
133  int res=0;
134  int ip1[2], ip2[2];
135  unsigned int uip1[2], uip2[2];
136  if (p1.IsAtInfinity() || p2.IsAtInfinity())
137  res=-2;
138  else {
139  ip1[0]=(int)rint(p1[0]/p1[2]);
140  ip1[1]=(int)rint(p1[1]/p1[2]);
141  ip2[0]=(int)rint(p2[0]/p2[2]);
142  ip2[1]=(int)rint(p2[1]/p2[2]);
143  if (ip1[0]<(int)halfwinsize ||
144  ip1[0]+(int)halfwinsize>=(int)im1.GetWidth() ||
145  ip1[1]<(int)halfwinsize ||
146  ip1[1]+(int)halfwinsize>=(int)im1.GetHeight() ||
147  ip2[0]<(int)halfwinsize ||
148  ip2[0]+(int)halfwinsize>=(int)im2.GetWidth() ||
149  ip2[1]<(int)halfwinsize ||
150  ip2[1]+(int)halfwinsize>=(int)im2.GetHeight())
151  res=-1;
152  else{
153  uip1[0]=(unsigned int)ip1[0];
154  uip1[1]=(unsigned int)ip1[1];
155  uip2[0]=(unsigned int)ip2[0];
156  uip2[1]=(unsigned int)ip2[1];
157  RegionMatcher::NCC(uip1, uip2, im1.GetImageDataArray(),
158  im2.GetImageDataArray(), halfwinsize, result);
159  }
160  }
161  return res;
162 }
163 
164 
165 template <class StorageType>
166 int CornerMatcher::
167 NCCSearch(const HomgPoint2D& p1, const HomgPoint2D& p2,
168  const Image<StorageType>& im1, const Image<StorageType>& im2,
169  const unsigned int halfnccwinsize,
170  const unsigned int halfsearchwinsize,
171  HomgPoint2D& resultpoint,
172  const double mincorrelation,
173  double& result )
174 {
175  BIASASSERT(im1.GetColorModel()==BIAS::ImageBase::CM_Grey);
176  BIASASSERT(im2.GetColorModel()==BIAS::ImageBase::CM_Grey);
177 
178  const double THRESHOLD=1e-8;
179  int res=0;
180  int ip1[2], ip2[2];
181  unsigned int uip1[2], uip2[2], resp[2];
182  int minedgedist = halfnccwinsize + halfsearchwinsize;
183  if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
184  res=-2;
185  } else {
186  ip1[0]=(int)rint(p1[0]/p1[2]);
187  ip1[1]=(int)rint(p1[1]/p1[2]);
188  ip2[0]=(int)rint(p2[0]/p2[2]);
189  ip2[1]=(int)rint(p2[1]/p2[2]);
190  if (fabs(p2[0]/p2[2]-ip2[0])<THRESHOLD &&
191  fabs(p2[1]/p2[2]-ip2[1])<THRESHOLD )
192  {
193  if (ip1[0] <= minedgedist || ip1[0] + minedgedist>=(int)im1.GetWidth() ||
194  ip1[1] <= minedgedist || ip1[1] + minedgedist>=(int)im1.GetHeight() ||
195  ip2[0] <= minedgedist || ip2[0] + minedgedist>=(int)im2.GetWidth() ||
196  ip2[1] <= minedgedist || ip2[1] + minedgedist>=(int)im2.GetHeight() ){
197  res=-1;
198  } else {
199  uip1[0]=(unsigned int)ip1[0];
200  uip1[1]=(unsigned int)ip1[1];
201  uip2[0]=(unsigned int)ip2[0];
202  uip2[1]=(unsigned int)ip2[1];
203  BIASDOUT(D_CORNER_MATCHER_SEARCH,"p1 " << p1 << " " << uip1[0] << " "
204  << uip1[1] << "\tp2 " << p2 << " " << uip2[0]<<" "<< uip2[1]);
205  BIASDOUT(D_CORNER_MATCHER_SEARCH,"using NCCSearchOdd for " << p2
206  << " " << uip2[0] << " "<< uip2[1]);
207  res = NCCSearchOdd(uip1, uip2, im1.GetImageDataArray(),
208  im2.GetImageDataArray(), halfnccwinsize,
209  halfsearchwinsize, resp, mincorrelation, result);
210  resultpoint.Set((double)resp[0], (double)resp[1]);
211  BIASDOUT(D_CORNER_MATCHER_SEARCH,"resultpoint " << resultpoint);
212  } // within image
213  } else { // searcheven/searchodd
214  ip1[0]=(int)rint(p1[0]/p1[2]);
215  ip1[1]=(int)rint(p1[1]/p1[2]);
216  ip2[0]=(int)floor(p2[0]/p2[2]);
217  ip2[1]=(int)floor(p2[1]/p2[2]);
218  if (ip1[0] < minedgedist || ip1[0] + minedgedist>=(int)im1.GetWidth() ||
219  ip1[1] < minedgedist || ip1[1] + minedgedist>=(int)im1.GetHeight() ||
220  ip2[0] < minedgedist || ip2[0] + minedgedist>=(int)im2.GetWidth() ||
221  ip2[1] < minedgedist || ip2[1] + minedgedist>=(int)im2.GetHeight() ){
222  res=-1;
223  } else {
224  uip1[0]=(unsigned int)ip1[0];
225  uip1[1]=(unsigned int)ip1[1];
226  uip2[0]=(unsigned int)ip2[0];
227  uip2[1]=(unsigned int)ip2[1];
228  BIASDOUT(D_CORNER_MATCHER_SEARCH,"using NCCSearchEven for " << p2
229  << " " << uip2[0] << " "<< uip2[1]);
230  res = NCCSearchEven(uip1, uip2, im1.GetImageDataArray(),
231  im2.GetImageDataArray(), halfnccwinsize,
232  halfsearchwinsize, resp, mincorrelation, result);
233  resultpoint.Set((double)resp[0], (double)resp[1]);
234  BIASDOUT(D_CORNER_MATCHER_SEARCH,"resultpoint " << resultpoint);
235  } // within image
236  } // if searcheven | serachodd
237  } // if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
238  return res;
239 }
240 
241 /*
242 template <class StorageType>
243 int CornerMatcher::KLT(HomgPoint2D& p1, HomgPoint2D& p2,
244  Image<StorageType>& im1, Image<StorageType>& im2,
245  Image<StorageType>& gradx, Image<StorageType>& grady,
246  unsigned int halfwinsize, unsigned int gradientborder,
247  unsigned int numiterations, HomgPoint2D& result,
248  KLT_TYPE& error)
249 {
250  int res=0;
251  KLT_TYPE dp1[2], dp2[2], resp[2];
252  double minedgedist = (double)(halfwinsize + gradientborder);
253 
254  if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
255  res=-2;
256  } else {
257  dp1[0]=p1[0]/p1[2];
258  dp1[1]=p1[1]/p1[2];
259  dp2[0]=p2[0]/p2[2];
260  dp2[1]=p2[1]/p2[2];
261  if (dp1[0] < minedgedist || dp1[0] + minedgedist >= im1.GetWidth() ||
262  dp1[1] < minedgedist || dp1[1] + minedgedist >= im1.GetHeight() ||
263  dp2[0] < minedgedist || dp2[0] + minedgedist >= im2.GetWidth() ||
264  dp2[1] < minedgedist || dp2[1] + minedgedist >= im2.GetHeight() ) {
265  res=-1;
266  } else {
267  BIASDOUT(D_CORNER_MATCHER_KLT,"p1: " << p1 << "\tp2: "<< p2);
268  res = RegionMatcher::
269  KLT(dp1, dp2, im1.GetImageDataArray(), im2.GetImageDataArray(),
270  gradx.GetImageDataArray(), grady.GetImageDataArray(),
271  im1.GetWidth(), im1.GetHeight(), halfwinsize,
272  numiterations, resp, error);
273  result.Set(resp[0], resp[1]);
274  } // p1 / p1 within image
275  } // p1 || p2 at infinity
276 
277  return res;
278 }
279 
280 template <class StorageType>
281 int CornerMatcher::KLT(HomgPoint2D& p1, HomgPoint2D& p2,
282  Image<StorageType>& im1, Image<StorageType>& im2,
283  Image<StorageType>& gradx, Image<StorageType>& grady,
284  unsigned int halfwinsize, unsigned int gradientborder,
285  unsigned int maxnumiterations, KLT_TYPE maxerror,
286  HomgPoint2D& result, KLT_TYPE& error)
287 {
288  int res=0;
289  KLT_TYPE dp1[2], dp2[2], resp[2];
290  double minedgedist = (double)(halfwinsize + gradientborder);
291 
292  if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
293  BIASDOUT(D_CORNER_MATCHER_KLT," a point is at infinty p1: "
294  << p1 << "\tp2: "<< p2);
295  res=-2;
296  } else {
297  dp1[0]=p1[0]/p1[2];
298  dp1[1]=p1[1]/p1[2];
299  dp2[0]=p2[0]/p2[2];
300  dp2[1]=p2[1]/p2[2];
301  if (dp1[0] < minedgedist || dp1[0] + minedgedist >= im1.GetWidth() ||
302  dp1[1] < minedgedist || dp1[1] + minedgedist >= im1.GetHeight() ||
303  dp2[0] < minedgedist || dp2[0] + minedgedist >= im2.GetWidth() ||
304  dp2[1] < minedgedist || dp2[1] + minedgedist >= im2.GetHeight() ) {
305  BIASDOUT(D_CORNER_MATCHER_KLT,
306  " a point is to cloose to image border p1: "
307  << p1 << "\tp2: "<< p2);
308  res=-1;
309  } else {
310  BIASDOUT(D_CORNER_MATCHER_KLT,"p1: " << p1 << "\tp2: "<< p2);
311  res = RegionMatcher::
312  KLT(dp1, dp2, im1.GetImageDataArray(), im2.GetImageDataArray(),
313  gradx.GetImageDataArray(), grady.GetImageDataArray(),
314  im1.GetWidth(), im1.GetHeight(), halfwinsize,
315  maxnumiterations, maxerror, resp, error);
316  result.Set(resp[0], resp[1]);
317  } // p1 / p1 within image
318  } // p1 || p2 at infinity
319 
320  return res;
321 }
322 
323 template <class StorageType>
324 int CornerMatcher::KLT2(KLT_TYPE point1[2], KLT_TYPE point2[2],
325  PyramidImage2<StorageType>& pyim1,
326  PyramidImage2<StorageType>& pyim2,
327  PyramidImage2<StorageType>& pygradx1,
328  PyramidImage2<StorageType>& pygrady1,
329  PyramidImage2<StorageType>& pygradx2,
330  PyramidImage2<StorageType>& pygrady2, unsigned pyindex,
331  unsigned hws, unsigned maxiterations, double SADThresh,
332  KLT_TYPE maxerror, KLT_TYPE res[2], KLT_TYPE& error)
333 {
334 #ifdef TIMING
335  TimeMeasure timer;
336  timer.Start();
337 #endif
338  int pysize, width, height, j;
339  StorageType **ida1, **ida2, **idagx1, **idagy1, **idagx2, **idagy2;
340  unsigned x1, x2, y1, y2, corr, mh, mw, sadthresh3x3, sadthresh5x5;
341  KLT_TYPE p1[2], p2[2], fx, fy, div;
342 
343  sadthresh3x3=(unsigned)(SADThresh*9.0);
344  sadthresh5x5=(unsigned)(SADThresh*25.0);
345  pysize=pyim1.size();
346  // first track on smallest level
347  j=pysize-1;
348  ida1=pyim1[j]->GetImageDataArray();
349  ida2=pyim2[j]->GetImageDataArray();
350  idagx1=pygradx1[j]->GetImageDataArray();
351  idagy1=pygrady1[j]->GetImageDataArray();
352  idagx2=pygradx2[j]->GetImageDataArray();
353  idagy2=pygrady2[j]->GetImageDataArray();
354  width=pyim1[j]->GetWidth();
355  height=pyim1[j]->GetHeight();
356  mh=height-hws-1; mw=width-hws-1;
357 
358  // now track smalles level
359 #ifdef WIN32
360  // jw, sunmin: (09/2004) do NOT use std:: because of compex instance
361  div=1.0/pow(2.0, double(pysize-pyindex-1));
362 #else
363  // jw, woelk: (09/2004): Felix is *sure* that he needs std:: ;-)
364  div=1.0/std::pow(2.0, double(pysize-pyindex-1));
365 #endif
366 
367  p1[0]=point1[0]*div;
368  p1[1]=point1[1]*div;
369  p2[0]=p1[0]; p2[1]=p1[1];
370  fx=fy=0.0;
371  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" maybe tracking between ("<<p1[0]<<", "
372  <<p1[1]<<") and ("<<p2[0]<<", "<<p2[1]<<")\n");
373  if (p2[0]>hws && p2[1]>hws && p2[0]<mw && p2[1]<mh){
374  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" tracking between ("<<p1[0]<<", "
375  <<p1[1]<<") and ("<<p2[0]<<", "<<p2[1]<<")\n");
376  if (RegionMatcher::KLT2(p1, p2,ida1,ida2,idagx1,idagy1,idagx2,idagy2,width,
377  height,hws,maxiterations,maxerror, res, error)==0){
378  BIASCDOUT(D_CORNER_MATCHER_PYKLT,j<<" tracking succeeded, maybe sad "
379  <<"between ("<<p1[0]<<", "<<p1[1]<<") and ("<<res[0]<<", "
380  <<res[1]<<")\n");
381  x1=(unsigned)rint(p1[0]);
382  y1=(unsigned)rint(p1[1]);
383  x2=(unsigned)rint(res[0]);
384  y2=(unsigned)rint(res[1]);
385  if (x2>=2 && y2>=2 && x2<mw && y2<mh){
386  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad between ("
387  <<x1<<", "<<y1<<") and ("<<x2<<", "<<y2<<")\n");
388  SAD3x3(x1, y1, x2, y2, ida1, ida2, corr);
389  } else {
390  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" no sad, out of bounds\n");
391  if (j==(int)pyindex) {
392  return -1;
393  } else {
394  corr=sadthresh3x3; // cannot use sad so fake failure
395  }
396  }
397  if (corr<sadthresh3x3){
398  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad succeeded ("<<p1[0]<<", "<<
399  p1[1]<<") and ("<<res[0]<<", "<<res[1]<<") = "<<corr<<"\n");
400  fx=res[0]-p1[0];
401  fy=res[1]-p1[1];
402  } else {
403  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad failed ("<<p1[0]<<", "<<
404  p1[1]<<") and ("<<res[0]<<", "<<res[1]<<") = "<<corr<<"\n");
405  fx=fy=0.0;
406  if (j==(int)pyindex) { return -2; }
407  }
408  } else {
409  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" KLT failed\n");
410  if (j==(int)pyindex) { return -3; }
411  }
412  } else {
413  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" not tracking, out of bounds\n");
414  if (j==(int)pyindex) { return -4; }
415  }
416 
417 #ifdef TIMING
418  timer.Stop();
419  GetDebugStream() << "smallest pyramide leveltook "<<timer.GetRealTime()<<" us"<<endl;
420  timer.Reset();
421  timer.Start();
422 #endif
423  // hws=2;
424  for (j=pysize-2; j>=(int)pyindex; j--){
425  ida1=pyim1[j]->GetImageDataArray();
426  ida2=pyim2[j]->GetImageDataArray();
427  idagx1=pygradx1[j]->GetImageDataArray();
428  idagy1=pygrady1[j]->GetImageDataArray();
429  idagx2=pygradx2[j]->GetImageDataArray();
430  idagy2=pygrady2[j]->GetImageDataArray();
431  width=pyim1[j]->GetWidth();
432  height=pyim1[j]->GetHeight();
433  mh=height-hws-1; mw=width-hws-1;
434 
435  if (j==(int)pyindex){
436  p1[0]=point1[0];
437  p1[1]=point1[1];
438  } else {
439  p1[0]*=2.0;
440  p1[1]*=2.0;
441  }
442  p2[0]=p1[0]+fx*2;
443  p2[1]=p1[1]+fy*2;
444 
445  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" maybe tracking between ("<<p1[0]
446  <<", "<<p1[1]<<") and ("<<p2[0]<<", "<<p2[1]<<")\n");
447  if (p2[0]>hws && p2[1]>hws && p2[0]<mw && p2[1]<mh){
448  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" tracking between ("<<p1[0]<<", "
449  <<p1[1]<<") and ("<<p2[0]<<", "<<p2[1]<<")\n");
450  if (RegionMatcher::KLT2(p1,p2,ida1,ida2,idagx1,idagy1,idagx2,idagy2,
451  width,height,hws,maxiterations,maxerror,
452  res,error)==0){
453  BIASCDOUT(D_CORNER_MATCHER_PYKLT,j<<" tracking succeeded, maybe sad "
454  <<"between ("<<p1[0]<<", "<<p1[1]<<") and ("<<res[0]<<", "
455  <<res[1]<<")\n");
456  x1=(unsigned)rint(p1[0]);
457  y1=(unsigned)rint(p1[1]);
458  x2=(unsigned)rint(res[0]);
459  y2=(unsigned)rint(res[1]);
460 
461  if (x2>=3 && y2>=3 && x2<mw && y2<mh){
462  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad between ("
463  <<x1<<", "<<y1<<") and ("<<x2<<", "<<y2<<")\n");
464  SAD5x5(x1, y1, x2, y2, ida1, ida2, corr);
465  } else {
466  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" no sad, out of bounds\n");
467  if (j==(int)pyindex) {
468  return -1;
469  } else {
470  corr=sadthresh5x5;
471  } // cannot use sad so fake failure
472  }
473  if (corr<sadthresh5x5){
474  fx=res[0]-p1[0];
475  fy=res[1]-p1[1];
476  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad succeeded, adding ("<<
477  p1[0]<<", "<<p1[1]<<") -> ("<<res[0]<<", "<<res[1]<<"\n");
478  } else {
479  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" sad failed: "<<corr<<"\n");
480  if (j==(int)pyindex) { return -2; }
481  else { fx*=2.0; fy*=2.0; }
482  }
483  } else {
484  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" KLT failed\n");
485  if (j==(int)pyindex) { return -3; }
486  }
487  } else {
488  BIASCDOUT(D_CORNER_MATCHER_PYKLT, j<<" not tracking, out of bounds\n");
489  if (j==(int)pyindex) { return -4; }
490  }
491  }
492 #ifdef TIMING
493  timer.Stop();
494  GetDebugStream() << "Pyramid KLT2 took "<<timer.GetRealTime()<<" us"<<endl;
495 #endif
496  return 0;
497 
498 }
499 */
500 template <class StorageType>
501 int CornerMatcher::
502 ParabolaNCC(const HomgPoint2D& p1, const HomgPoint2D& p2,
503  const Image<StorageType>& im1, const Image<StorageType>& im2,
504  const unsigned int halfwinsize,HomgPoint2D& result) const
505 {
506  int res=0;
507  KLT_TYPE resu[2];
508  unsigned int uip1[2], uip2[2];
509  double dp1x, dp1y;
510  int ip1[2], ip2[2];
511  int minedgedist = halfwinsize + 1;
512  if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
513  res=-2;
514  } else {
515  ip1[0]=(int)rint(p1[0]/p1[2]);
516  ip1[1]=(int)rint(p1[1]/p1[2]);
517  ip2[0]=(int)rint(p2[0]/p2[2]);
518  ip2[1]=(int)rint(p2[1]/p2[2]);
519  if (ip1[0] < minedgedist || ip1[0] + minedgedist>=(int)im1.GetWidth() ||
520  ip1[1] < minedgedist || ip1[1] + minedgedist>=(int)im1.GetHeight() ||
521  ip2[0] < minedgedist || ip2[0] + minedgedist>=(int)im2.GetWidth() ||
522  ip2[1] < minedgedist || ip2[1] + minedgedist>=(int)im2.GetHeight() ){
523  res=-1;
524  } else {
525  uip1[0]=(unsigned int)ip1[0];
526  dp1x=p1[0]-(double)uip1[0];
527  uip1[1]=(unsigned int)ip1[1];
528  dp1y=p1[1]-(double)uip1[1];
529  uip2[0]=(unsigned int)ip2[0];
530  uip2[1]=(unsigned int)ip2[1];
531  res=RegionMatcher::ParabolaNCC(uip1, uip2, im1.GetImageDataArray(),
532  im2.GetImageDataArray(), halfwinsize,
533  resu);
534  result.Set(resu[0]+dp1x, resu[1]+dp1y);
535  BIASDOUT(D_CORNER_MATCHER_PARABOLA, "\np1 "<<p1<<"\tp2 "<<p2
536  <<"\tuip1 "<<uip1[0]<<" "<<uip1[1]<<"\tuip2 "<<uip2[0]
537  <<" "<<uip2[1]<<"\tresu: "<<resu[0]<<" "<<resu[1]
538  <<"\tdp1x/y "<<dp1x<<" "<<dp1y);
539  }
540  }
541  return res;
542 }
543 
544 
545 template <class StorageType>
546 int CornerMatcher::
547 ParabolaNCC5(const HomgPoint2D& p1, const HomgPoint2D& p2,
548  const Image<StorageType>& im1, const Image<StorageType>& im2,
549  const unsigned int halfwinsize, HomgPoint2D& result)const
550 {
551  int res=0;
552  KLT_TYPE resu[2];
553  unsigned int uip1[2], uip2[2];
554  double dp1x, dp1y;
555  int ip1[2], ip2[2];
556  int minedgedist = halfwinsize + 2;
557  if (p1.IsAtInfinity() || p2.IsAtInfinity()) {
558  res=-2;
559  } else {
560  ip1[0]=(int)rint(p1[0]/p1[2]);
561  ip1[1]=(int)rint(p1[1]/p1[2]);
562  ip2[0]=(int)rint(p2[0]/p2[2]);
563  ip2[1]=(int)rint(p2[1]/p2[2]);
564  if (ip1[0] < minedgedist || ip1[0] + minedgedist>=(int)im1.GetWidth() ||
565  ip1[1] < minedgedist || ip1[1] + minedgedist>=(int)im1.GetHeight() ||
566  ip2[0] < minedgedist || ip2[0] + minedgedist>=(int)im2.GetWidth() ||
567  ip2[1] < minedgedist || ip2[1] + minedgedist>=(int)im2.GetHeight() ){
568  res=-1;
569  } else {
570  uip1[0]=(unsigned int)ip1[0];
571  dp1x=p1[0]-(double)uip1[0];
572  uip1[1]=(unsigned int)ip1[1];
573  dp1y=p1[1]-(double)uip1[1];
574  uip2[0]=(unsigned int)ip2[0];
575  uip2[1]=(unsigned int)ip2[1];
576  res=RegionMatcher::ParabolaNCC5(uip1, uip2, im1.GetImageDataArray(),
577  im2.GetImageDataArray(), halfwinsize,
578  resu);
579  result.Set(resu[0]+dp1x, resu[1]+dp1y);
580  BIASDOUT(D_CORNER_MATCHER_PARABOLA, "\np1 "<<p1<<"\tp2 "<<p2
581  <<"\tuip1 "<<uip1[0]<<" "<<uip1[1]<<"\tuip2 "<<uip2[0]
582  <<" "<<uip2[1]<<"\tresu: "<<resu[0]<<" "<<resu[1]
583  <<"\tdp1x/y "<<dp1x<<" "<<dp1y);
584  }
585  }
586  return res;
587 }
588 
589 
590 template <class StorageType>
591 int CornerMatcher::
592 RefinePyramideNCC(vector<vector<HomgPoint2D> >& ps1,
593  PyramidImage<StorageType>& im1,
594  PyramidImage<StorageType>& im2,
595  unsigned int halfnccwinsize,
596  vector<vector<HomgPoint2D> >& matches,
597  vector<vector<double> >& matchquality)
598 {
599  int res=0;
600  unsigned p1[2], p2[2];
601  const StorageType **ida1, **ida2;
602 
603  for (int lev=im1.size()-2; lev>=0; lev--){
604  matches[lev].resize(ps1[lev].size());
605  matchquality[lev].resize(ps1[lev].size());
606  ida1=(const StorageType **)im1[lev]->GetImageDataArray();
607  ida2=(const StorageType **)im2[lev]->GetImageDataArray();
608  unsigned maxx=im1[lev]->GetWidth()-halfnccwinsize-1;
609  unsigned maxy=im1[lev]->GetHeight()-halfnccwinsize-1;
610  for (unsigned i=0; i<ps1[lev].size(); i++){
611  p1[0]=(unsigned int)rint(ps1[lev][i][0]);
612  p1[1]=(unsigned int)rint(ps1[lev][i][1]);
613  p2[0]=((unsigned int)rint(matches[lev+1][i][0]))<<1;
614  p2[1]=((unsigned int)rint(matches[lev+1][i][1]))<<1;
615  if (p1[0]>halfnccwinsize && p1[0]<maxx &&
616  p1[1]>halfnccwinsize && p1[1]<maxy &&
617  p2[0]>halfnccwinsize && p2[0]<maxx &&
618  p2[1]>halfnccwinsize && p2[1]<maxy &&
619  matchquality[lev+1][i] > -1.0){
620  NCCSearchEven(p1, p2, ida1, ida2, halfnccwinsize, 3, p2,
621  1.0, matchquality[lev][i]);
622  } else {
623  matchquality[lev][i]=-5.0;
624  }
625  matches[lev][i].Set((double)p2[0], (double)p2[1]);
626  /* cerr << lev << " : "<<setw(4)<<i<<" : "<<ps1[lev][i]
627  <<" <--> "<<matches[lev][i]
628  << "\t@ "<<matchquality[lev][i]<<"\t"<<p2[0]<<", "<<p2[1]<<endl;*/
629  } // for (ps1it=ps1[lev].begin(); ps1it!=ps1[lev].end(); ps1it++){
630  } // for (int lev=small-1; lev>=0; lev--)
631  return res;
632 }
633 
BIAS::HomgPoint2D
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
BIAS::CornerMatcher::RefinePyramideNCC
int RefinePyramideNCC(std::vector< std::vector< HomgPoint2D > > &ps1, PyramidImage< StorageType > &im1, PyramidImage< StorageType > &im2, unsigned int halfnccwinsize, std::vector< std::vector< HomgPoint2D > > &matches, std::vector< std::vector< double > > &matchquality)
Assumes that ps1 is fully filled.
Definition: CornerMatcher.cpp:592
BIAS::ImageBase::CM_Grey
gray values, 1 channel
Definition: ImageBase.hh:130
BIAS::PyramidImageInterface::size
unsigned size() const
deprecated interface
Definition: PyramidImageInterface.hh:102
BIAS::CornerMatcher::SSD
int SSD(const HomgPoint2D &p1, const HomgPoint2D &p2, const Image< StorageType > &im1, const Image< StorageType > &im2, const unsigned int halfwinsize, double &result) const
wrapper for SSD from RegionMatcher, does boundary checks and casts uses rint to determine location of...
Definition: CornerMatcher.cpp:92
BIAS::ImageBase::GetWidth
unsigned int GetWidth() const
Definition: ImageBase.hh:312
BIAS::RegionMatcher::NCCSearchEven
int NCCSearchEven(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfnccwinsize, const unsigned int halfsearchwinsize, unsigned int resultpoint[2], const double mincorrelation, double &result) const
As above but middle point of searchwindow is p1[0]+0.5, p1[1]+0.5 Searchwindow is always even-sized f...
Definition: RegionMatcher.cpp:342
BIAS::CornerMatcher::NCC
int NCC(const HomgPoint2D &p1, const HomgPoint2D &p2, const Image< StorageType > &im1, const Image< StorageType > &im2, const unsigned int halfwinsize, double &result) const
wrapper for NCC from RegionMatcher, does boundary checks and casts uses rint to determine location of...
Definition: CornerMatcher.cpp:129
BIAS::RegionMatcher::ParabolaNCC
int ParabolaNCC(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfwinsize, KLT_TYPE result[2]) const
function to achieve sub-pixel accuracy if p1 and p2 are corresponding image points from ida1 resp ida...
Definition: RegionMatcher.cpp:610
BIAS::RegionMatcher::NCCSearchOdd
int NCCSearchOdd(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfnccwinsize, const unsigned int halfsearchwinsize, unsigned int resultpoint[2], const double mincorrelation, double &result) const
Searches in a window around p2 [p2[i]+-halfsearchwinsize] for a point with correlation &gt; mincorrelati...
Definition: RegionMatcher.cpp:237
BIAS::CornerMatcher::NCCSearch
int NCCSearch(const HomgPoint2D &p1, const HomgPoint2D &p2, const Image< StorageType > &im1, const Image< StorageType > &im2, const unsigned int halfnccwinsizeconst, const unsigned int halfsearchwinsize, HomgPoint2D &resultpoint, const double mincorrelation, double &result)
wrapper for RegionMatcher::NCCSearchOdd/NCCSearchEven, does boundary checks and casts ...
Definition: CornerMatcher.cpp:167
BIAS::HomgPoint2D::IsAtInfinity
bool IsAtInfinity() const
Definition: HomgPoint2D.hh:165
BIAS::ImageBase::GetHeight
unsigned int GetHeight() const
Definition: ImageBase.hh:319
BIAS::RegionMatcher::SSD
void SSD(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfwinsize, double &result) const
fast sum of square differences (SSD) sim.
Definition: SimilarityMeasure.cpp:148
BIAS::INST
INST(unsigned char)
BIAS::Image
The image template class for specific storage types.
Definition: Image.hh:78
BIAS::RegionMatcher
Basic functions for CornerMatcher.
Definition: RegionMatcher.hh:151
BIAS::ImageBase::GetColorModel
enum EColorModel GetColorModel() const
Definition: ImageBase.hh:407
BIAS::CornerMatcher::ParabolaNCC
int ParabolaNCC(const HomgPoint2D &p1, const HomgPoint2D &p2, const Image< StorageType > &im1, const Image< StorageType > &im2, const unsigned int halfwinsize, HomgPoint2D &result) const
wrapper with range checking for RegionMatcher::KLT for a description see RegionMatcher::KLT gradientb...
Definition: CornerMatcher.cpp:502
BIAS::CornerMatcher::ParabolaNCC5
int ParabolaNCC5(const HomgPoint2D &p1, const HomgPoint2D &p2, const Image< StorageType > &im1, const Image< StorageType > &im2, const unsigned int halfwinsize, HomgPoint2D &result) const
wrapper for RegionMatcher::ParabolaNCC5
Definition: CornerMatcher.cpp:547
BIAS::RegionMatcher::ParabolaNCC5
int ParabolaNCC5(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfwinsize, KLT_TYPE result[2]) const
function to achieve sub-pixel accuracy if p1 and p2 are corresponding image points from ida1 resp ida...
Definition: RegionMatcher.cpp:659
BIAS::HomgPoint2D::Set
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
Definition: HomgPoint2D.hh:174
BIAS::RegionMatcher::NCC
void NCC(const unsigned int p1[2], const unsigned int p2[2], const StorageType **ida1, const StorageType **ida2, const unsigned int halfwinsize, double &result) const
fast ncc between p1 and p2 without boundary checking Faster specialisation for unsigned char exists b...
Definition: SimilarityMeasure.cpp:187
BIAS::Image::GetImageDataArray
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase
Definition: Image.hh:153
Generated on Tue Feb 6 2018 02:01:40 for Basic Image AlgorithmS Library by   doxygen 1.8.5