CornerSubpixel.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: KLTTracker.cpp
37 // Author: Pedram Azad
38 // Date: 18.11.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 "CornerSubpixel.h"
49 #include "Image/ByteImage.h"
50 #include "Math/Math2d.h"
51 
52 #include <stdio.h>
53 #include <float.h>
54 #include <math.h>
55 
56 
57 // ****************************************************************************
58 // Defines
59 // ****************************************************************************
60 
61 #define MAX_CORNER_SUBPIXEL_ITERATIONS 100
62 
63 
64 
65 // ****************************************************************************
66 // Methods
67 // ****************************************************************************
68 
69 bool CornerSubpixel::Refine(const CByteImage *pImage, const Vec2d &point, Vec2d &resultPoint, int nHalfWindowSize, int nMaxIterations)
70 {
71  if (pImage->type != CByteImage::eGrayScale)
72  {
73  printf("error: image must be of type eGrayScale for CCornerSubpixel::refine\n");
74  return false;
75  }
76 
77  const int width = pImage->width;
78  const int height = pImage->height;
79 
80  const int nWindowSize = 2 * nHalfWindowSize + 1;
81  const int nWindowSizePlus2 = nWindowSize + 2;
82  const int diff = width - nWindowSizePlus2;
83 
84  float grayValues[1024];
85  float *pGrayValues = nWindowSizePlus2 <= 32 ? grayValues : new float[nWindowSizePlus2 * nWindowSizePlus2];
86 
87  // track each point
88  const float ux = point.x;
89  const float uy = point.y;
90 
91  const unsigned char *pixels = pImage->pixels;
92 
93  Vec2d v = { 0.0f, 0.0f };
94 
95  for (int k = 0; k < nMaxIterations; k++)
96  {
97  const int xb = int(floorf(ux + v.x));
98  const int yb = int(floorf(uy + v.y));
99 
100  if (xb - nHalfWindowSize < 1 || xb + nHalfWindowSize + 3 >= width || yb - nHalfWindowSize < 1 || yb + nHalfWindowSize + 3 >= height)
101  break;
102 
103  const float dx = ux + v.x - float(xb);
104  const float dy = uy + v.y - float(yb);
105 
106  const float f00 = (1.0f - dx) * (1.0f - dy);
107  const float f10 = dx * (1.0f - dy);
108  const float f01 = (1.0f - dx) * dy;
109  const float f11 = dx * dy;
110 
111  int j, offset, offset2;
112 
113  for (j = nWindowSizePlus2, offset = (yb - nHalfWindowSize - 1) * width + xb - nHalfWindowSize - 1, offset2 = 0; j; j--, offset += diff)
114  for (int jj = nWindowSizePlus2; jj; jj--, offset++, offset2++)
115  pGrayValues[offset2] = f00 * pixels[offset] + f10 * pixels[offset + 1] + f01 * pixels[offset + width] + f11 * pixels[offset + width + 1];
116 
117  float gxx_sum = 0.0f, gxy_sum = 0.0f, gyy_sum = 0.0f;
118  float bx = 0.0f, by = 0.0f;
119 
120  for (j = 0, offset = nWindowSizePlus2 + 1; j < nWindowSize; j++, offset += 2)
121  for (int jj = 0; jj < nWindowSize; jj++, offset++)
122  {
123  const float ix = pGrayValues[offset + 1] - pGrayValues[offset - 1];
124  const float iy = pGrayValues[offset + nWindowSizePlus2] - pGrayValues[offset - nWindowSizePlus2];
125 
126  const float gxx = ix * ix;
127  const float gyy = iy * iy;
128  const float gxy = ix * iy;
129 
130  gxx_sum += gxx;
131  gyy_sum += gyy;
132  gxy_sum += gxy;
133 
134  bx += gxx * float(jj - nHalfWindowSize) + gxy * float(j - nHalfWindowSize);
135  by += gxy * float(jj - nHalfWindowSize) + gyy * float(j - nHalfWindowSize);
136  }
137 
138  if (fabsf(gxx_sum * gyy_sum - gxy_sum * gxy_sum) <= FLT_EPSILON)
139  {
140  if (nWindowSizePlus2 > 32)
141  delete [] pGrayValues;
142 
143  Math2d::SetVec(resultPoint, point);
144 
145  return false;
146  }
147 
148  const Mat2d G = { gxx_sum, gxy_sum, gxy_sum, gyy_sum };
149  Mat2d G_;
150  Math2d::Invert(G, G_);
151 
152  const Vec2d b = { bx, by };
153 
154  Vec2d n;
155  Math2d::MulMatVec(G_, b, n);
156 
157  Math2d::AddToVec(v, n);
158 
159  if (Math2d::SquaredLength(n) < 0.03f)
160  break;
161  }
162 
163  if (nWindowSizePlus2 > 32)
164  delete [] pGrayValues;
165 
166  Math2d::SetVec(resultPoint, ux + v.x, uy + v.y);
167 
168  return true;
169 }
float y
Definition: Math2d.h:84
bool Refine(const CByteImage *pImage, const Vec2d &point, Vec2d &resultPoint, int nHalfWindowSize=2, int nMaxIterations=100)
int width
The width of the image in pixels.
Definition: ByteImage.h:257
GLenum GLsizei n
Definition: glext.h:4209
Data structure for the representation of 8-bit grayscale images and 24-bit RGB (or HSV) color images ...
Definition: ByteImage.h:80
GLintptr offset
Definition: glext.h:3389
unsigned char * pixels
The pointer to the the pixels.
Definition: ByteImage.h:283
float x
Definition: Math2d.h:84
Data structure for the representation of a 2x2 matrix.
Definition: Math2d.h:101
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
Definition: glext.h:3154
int height
The height of the image in pixels.
Definition: ByteImage.h:264
void Invert(const Mat2d &matrix, Mat2d &result)
Definition: Math2d.cpp:238
GLubyte GLubyte b
Definition: glext.h:5166
GLenum GLsizei width
Definition: glext.h:3122
GLenum GLsizei GLsizei height
Definition: glext.h:3132
void AddToVec(Vec2d &vec, const Vec2d &vectorToAdd)
Definition: Math2d.cpp:81
ImageType type
The type of the image.
Definition: ByteImage.h:292
const GLdouble * v
Definition: glext.h:3212
GLbyte by
Definition: glext.h:5042
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
void SetVec(Vec2d &vec, float x, float y)
Definition: Math2d.cpp:68
float SquaredLength(const Vec2d &vec)
Definition: Math2d.cpp:176
void MulMatVec(const Mat2d &matrix, const Vec2d &vec, Vec2d &result)
Definition: Math2d.cpp:128


asr_ivt
Author(s): Allgeyer Tobias, Hutmacher Robin, Kleinert Daniel, Meißner Pascal, Scholz Jonas, Stöckle Patrick
autogenerated on Mon Dec 2 2019 03:47:27