StereoMatcher.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: StereoMatcher.cpp
37 // Author: Pedram Azad
38 // Date: 12.03.2007
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "StereoMatcher.h"
49 
50 #include "Image/ByteImage.h"
53 #include "Math/Math2d.h"
54 #include "Math/Math3d.h"
55 
56 #include <stdlib.h>
57 #include <math.h>
58 #include <stdio.h>
59 #include <float.h>
60 
61 
62 
63 // ****************************************************************************
64 // Constructor / Destructor
65 // ****************************************************************************
66 
68 {
71 }
72 
74 {
76  delete m_pStereoCalibration;
77 }
78 
79 
80 // ****************************************************************************
81 // Methods
82 // ****************************************************************************
83 
84 bool CStereoMatcher::LoadCameraParameters(const char *pFileName)
85 {
88 
89  return m_pStereoCalibration->LoadCameraParameters(pFileName);
90 }
91 
92 void CStereoMatcher::InitCameraParameters(CStereoCalibration *pStereoCalibration, bool bCloneCalibration)
93 {
94  if (bCloneCalibration)
95  {
98 
99  m_pStereoCalibration->Set(*pStereoCalibration);
101  }
102  else
103  {
105  delete m_pStereoCalibration;
106 
107  m_pStereoCalibration = pStereoCalibration;
109  }
110 }
111 
113 {
114  const Vec3d z_vector = { 0, 0, z };
115  Vec3d p;
117 
118  Vec2d left, right;
121 
122  return int(fabsf(left.x - right.x));
123 }
124 
125 int CStereoMatcher::Match(const CByteImage *pLeftImage, const CByteImage *pRightImage, int x, int y, int nWindowSize,
126  int d1, int d2, Vec2d &result, Vec3d &result_3d, float fThreshold, bool bInputImagesAreUndistorted)
127 {
128  if (pLeftImage->width != pRightImage->width || pLeftImage->height != pRightImage->height ||
129  pLeftImage->type != CByteImage::eGrayScale || pRightImage->type != CByteImage::eGrayScale)
130  {
131  printf("error: input images do not match for CStereoMatcher::Match\n");
132  return -100000;
133  }
134 
135  float *values_ = new float[4 * pLeftImage->width];
136  float *values = values_ + 2 * pLeftImage->width;
137 
138  int best_d = SingleZNCC(pLeftImage, pRightImage, x, y, nWindowSize, d1 - 1, d2 + 1, values);
139 
140  if (best_d > -100000 && best_d >= d1 && best_d <= d2 && values[best_d] > fThreshold)
141  {
142  const float y0 = values[best_d - 1];
143  const float y1 = values[best_d];
144  const float y2 = values[best_d + 1];
145  const float xmin = (y0 - y2) / (2.0f * (y0 - 2.0f * y1 + y2));
146  const float disparity = fabsf(xmin) < 0.5f ? best_d + xmin : best_d;
147 
148  float m, c;
149  const Vec2d point_left = { float(x), float(y) };
151 
152  result.x = x - disparity;
153  result.y = m * (x - disparity) + c;
154 
155  m_pStereoCalibration->Calculate3DPoint(point_left, result, result_3d, false, !bInputImagesAreUndistorted);
156  }
157  else
158  {
159  // adjust return value if threshold constraint was not met
160  if (best_d > -100000 && best_d >= d1 && best_d <= d2)
161  best_d = -100003;
162  }
163 
164  delete [] values_;
165 
166  return best_d;
167 }
168 
169 int CStereoMatcher::MatchZSAD(const CByteImage *pLeftImage, const CByteImage *pRightImage, int x, int y, int nWindowSize,
170  int d1, int d2, Vec2d &result, Vec3d &result_3d, float fThreshold, bool bInputImagesAreUndistorted)
171 {
172  if (pLeftImage->width != pRightImage->width || pLeftImage->height != pRightImage->height ||
173  pLeftImage->type != CByteImage::eGrayScale || pRightImage->type != CByteImage::eGrayScale)
174  {
175  printf("error: input images do not match for CStereoMatcher::MatchSAD\n");
176  return -100000;
177  }
178 
179  float *values_ = new float[4 * pLeftImage->width];
180  float *values = values_ + 2 * pLeftImage->width;
181 
182  int best_d = SingleZSAD(pLeftImage, pRightImage, x, y, nWindowSize, d1 - 1, d2 + 1, values);
183 
184  if (best_d > -100000 && best_d >= d1 && best_d <= d2 && 255.0f * values[best_d] < fThreshold)
185  {
186  const float y0 = values[best_d - 1];
187  const float y1 = values[best_d];
188  const float y2 = values[best_d + 1];
189  const float xmin = (y0 - y2) / (2.0f * (y0 - 2.0f * y1 + y2));
190  const float disparity = fabsf(xmin) < 0.5f ? best_d + xmin : best_d;
191 
192  float m, c;
193  const Vec2d point_left = { float(x), float(y) };
195 
196  result.x = x - disparity;
197  result.y = m * (x - disparity) + c;
198 
199  m_pStereoCalibration->Calculate3DPoint(point_left, result, result_3d, false, !bInputImagesAreUndistorted);
200  }
201  else
202  {
203  // adjust return value if threshold constraint was not met
204  if (best_d > -100000 && best_d >= d1 && best_d <= d2)
205  best_d = -100003;
206  }
207 
208  delete [] values_;
209 
210  return best_d;
211 }
212 
213 
214 int CStereoMatcher::SingleZNCC(const CByteImage *pInputImageLeft, const CByteImage *pInputImageRight, int x, int y, int nWindowSize, int d1, int d2, float *values)
215 {
216  const int width = pInputImageLeft->width;
217  const int height = pInputImageLeft->height;
218  const int w2 = nWindowSize / 2;
219 
220  if (x < w2 || x >= width - w2 || y < w2 || y >= height - w2)
221  return -100001;
222 
223  const unsigned char *input_left = pInputImageLeft->pixels;
224  const unsigned char *input_right = pInputImageRight->pixels;
225  const int nVectorLength = nWindowSize * nWindowSize;
226 
227  float *vector1 = new float[nVectorLength];
228  float *vector2 = new float[nVectorLength];
229 
230  const int offset = (y - w2) * width + (x - w2);
231  const int diff = width - nWindowSize;
232  const Vec2d camera_point = { float(x), float(y) };
233 
234  // calculate mean value
235  float mean = 0.0f;
236  for (int yy = 0, offset2 = offset, offset3 = 0; yy < nWindowSize; yy++, offset2 += diff)
237  for (int x = 0; x < nWindowSize; x++, offset2++, offset3++)
238  {
239  vector1[offset3] = input_left[offset2];
240  mean += vector1[offset3];
241  }
242  mean /= nVectorLength;
243 
244  // apply additive and muliplicative normalization
245  int k;
246  float factor = 0.0f;
247  for (k = 0; k < nVectorLength; k++)
248  {
249  vector1[k] -= mean;
250  factor += vector1[k] * vector1[k];
251  }
252 
253  if (factor < 60 * nVectorLength)
254  {
255  delete [] vector1;
256  delete [] vector2;
257  return -100002;
258  }
259 
260  factor = 1.0f / sqrtf(factor);
261  for (k = 0; k < nVectorLength; k++)
262  vector1[k] *= factor;
263 
264  float best_value = -FLT_MAX;
265  int d, best_d = -100004;
266 
267  const int max_d = d2 < x ? d2 : x;
268 
269  float m, c;
271 
272  // determine correspondence
273  for (d = d1; d <= max_d; d++)
274  {
275  const int xx = x - d; // x-coordinate of center of window in right image
276  const int yy = int(m * xx + c + 0.5f); // y-coordinate of center of window in right image
277 
278  if (xx < w2 || xx >= width - w2 || yy < w2 || yy >= height - w2)
279  continue;
280 
281  const int offset_right = (yy - w2) * width + (xx - w2); // offset of top left corner of window in right image
282 
283  // determine mean value
284  float mean = 0.0f;
285  for (int y = 0, offset2 = offset_right, offset3 = 0; y < nWindowSize; y++, offset2 += diff)
286  for (int x = 0; x < nWindowSize; x++, offset2++, offset3++)
287  {
288  vector2[offset3] = input_right[offset2];
289  mean += vector2[offset3];
290 
291  }
292  mean /= nVectorLength;
293 
294  // apply additive and multiplicative normalization
295  float factor = 0.0f;
296  for (k = 0; k < nVectorLength; k++)
297  {
298  vector2[k] -= mean;
299  factor += vector2[k] * vector2[k];
300  }
301 
302  if (factor < 60 * nVectorLength)
303  continue;
304 
305  factor = 1.0f / sqrtf(factor);
306  for (k = 0; k < nVectorLength; k++)
307  vector2[k] *= factor;
308 
309  float value = 0.0f;
310  for (k = 0; k < nVectorLength; k++)
311  value += vector1[k] * vector2[k];
312 
313  // save correlation results for subpixel calculations
314  values[d] = value;
315 
316  // determine maximum correlation value
317  if (value > best_value)
318  {
319  best_value = value;
320  best_d = d;
321  }
322  }
323 
324  delete [] vector1;
325  delete [] vector2;
326 
327  return best_d;
328 }
329 
330 int CStereoMatcher::SingleZSAD(const CByteImage *pInputImageLeft, const CByteImage *pInputImageRight, int x, int y, int nWindowSize, int d1, int d2, float *values)
331 {
332  const int width = pInputImageLeft->width;
333  const int height = pInputImageLeft->height;
334  const int w2 = nWindowSize / 2;
335 
336  if (x < w2 || x >= width - w2 || y < w2 || y >= height - w2)
337  return -100001;
338 
339  const unsigned char *input_left = pInputImageLeft->pixels;
340  const unsigned char *input_right = pInputImageRight->pixels;
341  const int nVectorLength = nWindowSize * nWindowSize;
342 
343  int *vector1 = new int[nVectorLength];
344  int *vector2 = new int[nVectorLength];
345 
346  const int offset = (y - w2) * width + (x - w2);
347  const int diff = width - nWindowSize;
348  const Vec2d camera_point = { float(x), float(y) };
349 
350  // calculate mean value
351  int mean1 = 0;
352  for (int yy = 0, offset2 = offset, offset3 = 0; yy < nWindowSize; yy++, offset2 += diff)
353  for (int x = 0; x < nWindowSize; x++, offset2++, offset3++)
354  {
355  vector1[offset3] = input_left[offset2];
356  mean1 += vector1[offset3];
357  }
358  mean1 /= nVectorLength;
359 
360  // apply additive and muliplicative normalization
361  float best_value = FLT_MAX;
362  int d, best_d = -100004;
363 
364  const int max_d = d2 < x ? d2 : x;
365 
366  float m, c;
368 
369  // determine correspondence
370  for (d = d1; d <= max_d; d++)
371  {
372  const int xx = x - d; // x-coordinate of center of window in right image
373  const int yy = int(m * xx + c + 0.5f); // y-coordinate of center of window in right image
374 
375  if (xx < w2 || xx >= width - w2 || yy < w2 || yy >= height - w2)
376  continue;
377 
378  const int offset_right = (yy - w2) * width + (xx - w2); // offset of top left corner of window in right image
379 
380  // determine mean value
381  int mean2 = 0;
382  for (int y = 0, offset2 = offset_right, offset3 = 0; y < nWindowSize; y++, offset2 += diff)
383  for (int x = 0; x < nWindowSize; x++, offset2++, offset3++)
384  {
385  vector2[offset3] = input_right[offset2];
386  mean2 += vector2[offset3];
387 
388  }
389  mean2 /= nVectorLength;
390 
391  int value_int = 0;
392  for (int k = 0; k < nVectorLength; k++)
393  value_int += abs((vector1[k] - mean1) - (vector2[k] - mean2));
394  const float value = float(value_int) / (nVectorLength * 255.0f);
395 
396  // save correlation results for subpixel calculations
397  values[d] = value;
398 
399  // determine maximum correlation value
400  if (value < best_value)
401  {
402  best_value = value;
403  best_d = d;
404  }
405  }
406 
407  delete [] vector1;
408  delete [] vector2;
409 
410  return best_d;
411 }
bool LoadCameraParameters(const char *pCameraParameterFileName, bool bTransformLeftCameraToIdentity=true)
Initializes the stereo camera model, given a file path to a stereo camera parameter file...
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3144
GLdouble GLdouble z
Definition: glext.h:3343
float y
Definition: Math2d.h:84
void InitCameraParameters(CStereoCalibration *pStereoCalibration, bool bCloneCalibration)
Initializes the internally used camera model, given an instance of CStereoCalibration.
void CameraToWorldCoordinates(const Vec3d &cameraPoint, Vec3d &worldPoint) const
Transforms 3D camera coordinates to 3D world coordinates.
int width
The width of the image in pixels.
Definition: ByteImage.h:257
bool m_bOwnStereoCalibrationObject
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
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
CStereoCalibration * m_pStereoCalibration
const CCalibration * GetLeftCalibration() const
Access to the instance of CCalibration for the camera model of the left camera.
unsigned char * pixels
The pointer to the the pixels.
Definition: ByteImage.h:283
void Calculate3DPoint(const Vec2d &cameraPointLeft, const Vec2d &cameraPointRight, Vec3d &worldPoint, bool bInputImagesAreRectified, bool bUseDistortionParameters=true, PointPair3d *pConnectionLine=0)
Computes a 3D point, given a point correspondence in both images, by performing stereo triangulation...
GLenum GLint x
Definition: glext.h:3125
float x
Definition: Math2d.h:84
const GLubyte * c
Definition: glext.h:5181
int MatchZSAD(const CByteImage *pLeftImage, const CByteImage *pRightImage, int x, int y, int nWindowSize, int d1, int d2, Vec2d &result, Vec3d &result_3d, float fThreshold, bool bInputImagesAreUndistorted=false)
Computes a disparity estimated for a given z-distance using the Zero-Mean Sum of Absolute Differences...
int SingleZNCC(const CByteImage *pInputImage1, const CByteImage *pInputImage2, int x, int y, int nWindowSize, int d1, int d2, float *values)
GLsizei const GLfloat * value
Definition: glext.h:3538
Camera model and functions for a stereo camera system.
int SingleZSAD(const CByteImage *pInputImage1, const CByteImage *pInputImage2, int x, int y, int nWindowSize, int d1, int d2, float *values)
int height
The height of the image in pixels.
Definition: ByteImage.h:264
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:4584
~CStereoMatcher()
The destructor.
GLenum GLsizei width
Definition: glext.h:3122
GLenum GLsizei GLsizei height
Definition: glext.h:3132
int GetDisparityEstimate(const float z)
Computes a disparity estimated for a given z-distance.
ImageType type
The type of the image.
Definition: ByteImage.h:292
bool LoadCameraParameters(const char *pCameraParameterFileName)
Initializes the internally used camera model, given a file path to a camera parameter file...
GLenum GLint GLint y
Definition: glext.h:3125
CStereoMatcher()
The default constructor (and only constructor).
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
void CalculateEpipolarLineInRightImage(const Vec2d &pointInLeftImage, Vec3d &l)
Given an image point in the left image, computes the corresponding epipolar line in the right image...
GLfloat GLfloat p
Definition: glext.h:5178
int Match(const CByteImage *pLeftImage, const CByteImage *pRightImage, int x, int y, int nWindowSize, int d1, int d2, Vec2d &result, Vec3d &result_3d, float fThreshold, bool bInputImagesAreUndistorted=false)
Computes a disparity estimated for a given z-distance using the Zero-Mean Normalized Cross Correlatio...
void WorldToImageCoordinates(const Vec3d &worldPoint, Vec2d &imagePoint, bool bUseDistortionParameters=true) const
Transforms 3D world coordinates to 2D image coordinates.
void Set(const CStereoCalibration &stereoCalibration)
Initializes the stereo camera model, given an instance of CStereoCalibration.
const CCalibration * GetRightCalibration() const
Access to the instance of CCalibration for the camera model of the right camera.


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:28