StereoVision.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: StereoVision.cpp
37 // Author: Pedram Azad
38 // Date: 2005
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "StereoVision.h"
49 
50 #include "ImageProcessor.h"
51 #include "ByteImage.h"
53 #include "Math/FloatMatrix.h"
55 #include "Helpers/helpers.h"
56 
57 #include <stdio.h>
58 #include <stdlib.h>
59 #include <string.h>
60 #include <math.h>
61 #include <limits.h>
62 
63 
64 
65 // *****************************************************************
66 // Constructor / Destructor
67 // *****************************************************************
68 
70 {
71 }
72 
74 {
75 }
76 
77 
78 // ****************************************************************************
79 // Methods
80 // ****************************************************************************
81 
82 bool CStereoVision::Process(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage,
83  int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold)
84 {
85  if (pLeftImage->width != pRightImage->width || pLeftImage->height != pRightImage->height ||
86  pLeftImage->type != CByteImage::eGrayScale || pRightImage->type != CByteImage::eGrayScale ||
87  pDepthImage->width != pLeftImage->width || pDepthImage->height != pLeftImage->height ||
88  pDepthImage->type != CByteImage::eGrayScale)
89  {
90  printf("error: input images and/or output image do not match for CStereoVision::ProcessSSD\n");
91  return false;
92  }
93 
94  if (d_step == 0)
95  {
96  printf("error: d_step must not be zero for CStereoVision::ProcessSSD\n");
97  return false;
98  }
99 
100  if (d_step > 0 && d1 >= d2)
101  {
102  printf("error: if d_step > 0 then d2 must be greater than d1 for CStereoVision::ProcessSSD\n");
103  return false;
104  }
105 
106  if (d_step < 0 && d1 <= d2)
107  {
108  printf("error: if d_step < 0 then d1 must be greater than d2 for CStereoVision::ProcessSSD\n");
109  return false;
110  }
111 
112  ImageProcessor::Zero(pDepthImage);
113 
114  nErrorThreshold *= nWindowSize * nWindowSize;
115 
116  // adjust d2 so we can use != instead of <= or >=
117  int d;
118  if (d_step > 0)
119  for (d = d1; d <= d2; d += d_step);
120  else if (d_step < 0)
121  for (d = d1; d >= d2; d += d_step);
122  d2 = d + d_step;
123 
124  const int width = pLeftImage->width;
125  const int height = pLeftImage->height;
126 
127  const unsigned char *pLeftImageData = pLeftImage->pixels;
128  const unsigned char *pRightImageData = pRightImage->pixels;
129  unsigned char *pDepthImageData = pDepthImage->pixels + (nWindowSize / 2) * (width + 1);
130 
131  const int max_i = height - nWindowSize + 1;
132  const int max_j = width - nWindowSize + 1;
133 
134  const int start_j = MY_MAX(nWindowSize, d2);
135  const int diff = width - (max_j - start_j);
136  const int diff2 = width - nWindowSize;
137 
138  for (int i = 0, offset = start_j; i < max_i; i++, offset += diff)
139  {
140  for (int j = start_j; j < max_j; j++, offset++)
141  {
142  int best_error = INT_MAX;
143  int best_d = 0;
144 
145  // find correlation
146  for (int d = d1; d != d2; d += d_step)
147  {
148  int error = 0;
149 
150  for (int y = 0, offset2 = offset; y < nWindowSize; y++, offset2 += diff2)
151  for (int x = 0; x < nWindowSize; x++, offset2++)
152  error += abs(pLeftImageData[offset2] - pRightImageData[offset2 - d]);
153 
154  if (error < best_error)
155  {
156  best_error = error;
157  best_d = d;
158  }
159  }
160 
161  pDepthImageData[i * width + j] = best_error < nErrorThreshold ? best_d : 0;
162  }
163  }
164 
165  return true;
166 }
167 
168 
169 bool CStereoVision::ProcessFast(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage,
170  int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold)
171 {
172  if (pLeftImage->width != pRightImage->width || pLeftImage->height != pRightImage->height ||
173  pLeftImage->type != CByteImage::eGrayScale || pRightImage->type != CByteImage::eGrayScale ||
174  pDepthImage->width != pLeftImage->width || pDepthImage->height != pLeftImage->height ||
175  pDepthImage->type != CByteImage::eGrayScale)
176  {
177  printf("error: intput images and output image do not match in CStereoVision::ProcessFast\n");
178  return false;
179  }
180 
181  if (d_step == 0)
182  {
183  printf("error: d_step must not be zero for CStereoVision::ProcessFast\n");
184  return false;
185  }
186 
187  if (d_step > 0 && d1 >= d2)
188  {
189  printf("error: if d_step > 0 then d2 must be greater than d1 for CStereoVision::ProcessFast\n");
190  return false;
191  }
192 
193  if (d_step < 0 && d1 <= d2)
194  {
195  printf("error: if d_step < 0 then d1 must be greater than d2 for CStereoVision::ProcessFast\n");
196  return false;
197  }
198 
199  //OPTIMIZED_FUNCTION_HEADER_8(StereoVision, pLeftImage, pRightImage, pDepthImage, nWindowSize, d1, d2, d_step, nErrorThreshold)
200 
201  _ProcessFast(pLeftImage, pRightImage, pDepthImage, nWindowSize, d1, d2, d_step, nErrorThreshold);
202 
203  //OPTIMIZED_FUNCTION_FOOTER
204 
205  return true;
206 }
207 
208 
209 void CStereoVision::_ProcessFast(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage,
210  int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold)
211 {
212  ImageProcessor::Zero(pDepthImage);
213 
214  nErrorThreshold *= nWindowSize * nWindowSize;
215 
216  // adjust d2 so we can use != instead of <= or >=
217  int d;
218  if (d_step > 0)
219  for (d = d1; d <= d2; d += d_step);
220  else if (d_step < 0)
221  for (d = d1; d >= d2; d += d_step);
222  d2 = d + d_step;
223 
224  const int width = pLeftImage->width;
225  const int height = pLeftImage->height;
226  const int nPixels = width * height;
227 
228  // allocate memory
229  int *H_SUM = new int[nWindowSize * width];
230  int *V_SUM = new int[width];
231  int *SAD = new int[width];
232  int *MIN_SSAD = new int[width * height];
233 
234  for (int i = 0; i < nPixels; i++)
235  MIN_SSAD[i] = nErrorThreshold;
236 
237  const int window_offset = - (nWindowSize / 2) * (width + 1);
238  const int start_c = MY_MAX(nWindowSize, d2) + nWindowSize;
239 
240  const unsigned char *pLeftImageData = pLeftImage->pixels;
241  const unsigned char *pRightImageData = pRightImage->pixels;
242  unsigned char *pDepthImageData = pDepthImage->pixels + window_offset;
243 
244  // run correlation algorithm
245  for (d = d1; d != d2; d += d_step)
246  {
247  int c, r;
248 
249  // initialize
250  for (r = 0; r < nWindowSize; r++)
251  {
252  const unsigned char *pLeftImageData_helper = pLeftImageData + r * width;
253  const unsigned char *pRightImageData_helper = pRightImageData + r * width - d;
254 
255  H_SUM[r * width + start_c - 1] = 0;
256 
257  for (c = start_c - nWindowSize; c < start_c; c++)
258  {
259  SAD[c] = abs(pLeftImageData_helper[c] - pRightImageData_helper[c]);
260  H_SUM[r * width + start_c - 1] += SAD[c];
261  }
262 
263  for (c = start_c; c < width; c++)
264  {
265  SAD[c] = abs(pLeftImageData_helper[c] - pRightImageData_helper[c]);
266  H_SUM[r * width + c] = H_SUM[r * width + c - 1] + SAD[c] - SAD[c - nWindowSize];
267  }
268  }
269 
270  int *pMinHelper = MIN_SSAD + (nWindowSize - 1) * width; // for topmost_row
271  unsigned char *pDepthHelper = pDepthImageData + (nWindowSize - 1) * width; // for topmost_row
272  for (c = start_c - 1; c < width; c++)
273  {
274  V_SUM[c] = 0;
275  for (r = 0; r < nWindowSize; r++)
276  V_SUM[c] += H_SUM[r * width + c];
277 
278  // required for writing topmost row
279  // <topmost_row>
280  if (V_SUM[c] < pMinHelper[c])
281  {
282  pMinHelper[c] = V_SUM[c];
283  pDepthHelper[c] = abs(d);
284  }
285  // </topmost_row>
286  }
287 
288  // go
289  for (r = nWindowSize; r < height; r++)
290  {
291  const unsigned char *pLeftImageData_helper = pLeftImageData + r * width;
292  const unsigned char *pRightImageData_helper = pRightImageData + r * width - d;
293 
294  int *hsum_helper = H_SUM + (r % nWindowSize) * width;
295  int *pMinHelper = MIN_SSAD + r * width;
296  unsigned char *pDepthHelper = pDepthImageData + r * width;
297 
298  const int save = hsum_helper[start_c - 1]; // for leftmost_column
299  hsum_helper[start_c - 1] = 0;
300  for (c = start_c - nWindowSize; c < start_c; c++)
301  {
302  SAD[c] = abs(pLeftImageData_helper[c] - pRightImageData_helper[c]);
303  hsum_helper[start_c - 1] += SAD[c];
304  }
305 
306  // required for writing leftmost column
307  // <leftmost_column>
308  const int result = V_SUM[start_c - 1] = V_SUM[start_c - 1] + hsum_helper[start_c - 1] - save;
309  if (result < pMinHelper[start_c - 1])
310  {
311  pMinHelper[start_c - 1] = result;
312  pDepthHelper[start_c - 1] = abs(d);
313  }
314  // </leftmost_column>
315 
316  // core loop
317  for (c = start_c; c < width; c++)
318  {
319  SAD[c] = abs(pLeftImageData_helper[c] - pRightImageData_helper[c]);
320 
321  const int save = hsum_helper[c];
322  hsum_helper[c] = hsum_helper[c - 1] + SAD[c] - SAD[c - nWindowSize];
323  const int result = V_SUM[c] = V_SUM[c] + hsum_helper[c] - save;
324 
325  if (result < pMinHelper[c])
326  {
327  pMinHelper[c] = result;
328  pDepthHelper[c] = abs(d);
329  }
330  }
331  }
332  }
333 
334  // free memory
335  delete [] H_SUM;
336  delete [] V_SUM;
337  delete [] SAD;
338  delete [] MIN_SSAD;
339 }
bool Process(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage, int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold=50)
void _ProcessFast(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage, int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold)
int width
The width of the image in pixels.
Definition: ByteImage.h:257
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
#define MY_MAX(a, b)
Definition: helpers.h:63
unsigned char * pixels
The pointer to the the pixels.
Definition: ByteImage.h:283
bool ProcessFast(const CByteImage *pLeftImage, const CByteImage *pRightImage, CByteImage *pDepthImage, int nWindowSize, int d1, int d2, int d_step, int nErrorThreshold=50)
GLenum GLint x
Definition: glext.h:3125
const GLubyte * c
Definition: glext.h:5181
int height
The height of the image in pixels.
Definition: ByteImage.h:264
GLenum GLsizei width
Definition: glext.h:3122
GLenum GLsizei GLsizei height
Definition: glext.h:3132
ImageType type
The type of the image.
Definition: ByteImage.h:292
GLdouble GLdouble GLdouble r
Definition: glext.h:3227
GLenum GLint GLint y
Definition: glext.h:3125
void Zero(CByteImage *pImage, const MyRegion *pROI=0)
Sets all values in a CByteImage to zero.


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