94 if (bCloneCalibration)
114 const Vec3d z_vector = { 0, 0, z };
122 return int(fabsf(left.
x - right.
x));
126 int d1,
int d2,
Vec2d &result,
Vec3d &result_3d,
float fThreshold,
bool bInputImagesAreUndistorted)
131 printf(
"error: input images do not match for CStereoMatcher::Match\n");
135 float *values_ =
new float[4 * pLeftImage->
width];
138 int best_d =
SingleZNCC(pLeftImage, pRightImage, x, y, nWindowSize, d1 - 1, d2 + 1, values);
140 if (best_d > -100000 && best_d >= d1 && best_d <= d2 && values[best_d] > fThreshold)
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;
149 const Vec2d point_left = { float(x), float(y) };
152 result.
x = x - disparity;
153 result.
y = m * (x - disparity) + c;
160 if (best_d > -100000 && best_d >= d1 && best_d <= d2)
170 int d1,
int d2,
Vec2d &result,
Vec3d &result_3d,
float fThreshold,
bool bInputImagesAreUndistorted)
175 printf(
"error: input images do not match for CStereoMatcher::MatchSAD\n");
179 float *values_ =
new float[4 * pLeftImage->
width];
182 int best_d =
SingleZSAD(pLeftImage, pRightImage, x, y, nWindowSize, d1 - 1, d2 + 1, values);
184 if (best_d > -100000 && best_d >= d1 && best_d <= d2 && 255.0f * values[best_d] < fThreshold)
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;
193 const Vec2d point_left = { float(x), float(y) };
196 result.
x = x - disparity;
197 result.
y = m * (x - disparity) + c;
204 if (best_d > -100000 && best_d >= d1 && best_d <= d2)
218 const int w2 = nWindowSize / 2;
223 const unsigned char *input_left = pInputImageLeft->
pixels;
224 const unsigned char *input_right = pInputImageRight->
pixels;
225 const int nVectorLength = nWindowSize * nWindowSize;
227 float *vector1 =
new float[nVectorLength];
228 float *vector2 =
new float[nVectorLength];
230 const int offset = (y -
w2) * width + (x - w2);
231 const int diff = width - nWindowSize;
232 const Vec2d camera_point = { float(x), float(y) };
236 for (
int yy = 0, offset2 = offset, offset3 = 0; yy < nWindowSize; yy++, offset2 += diff)
237 for (
int x = 0; x < nWindowSize; x++, offset2++, offset3++)
239 vector1[offset3] = input_left[offset2];
240 mean += vector1[offset3];
242 mean /= nVectorLength;
247 for (k = 0; k < nVectorLength; k++)
250 factor += vector1[k] * vector1[k];
253 if (factor < 60 * nVectorLength)
260 factor = 1.0f / sqrtf(factor);
261 for (k = 0; k < nVectorLength; k++)
262 vector1[k] *= factor;
264 float best_value = -FLT_MAX;
265 int d, best_d = -100004;
267 const int max_d = d2 < x ? d2 :
x;
273 for (d = d1; d <= max_d; d++)
275 const int xx = x - d;
276 const int yy = int(m * xx + c + 0.5f);
278 if (xx < w2 || xx >= width - w2 || yy < w2 || yy >= height - w2)
281 const int offset_right = (yy -
w2) * width + (xx - w2);
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++)
288 vector2[offset3] = input_right[offset2];
289 mean += vector2[offset3];
292 mean /= nVectorLength;
296 for (k = 0; k < nVectorLength; k++)
299 factor += vector2[k] * vector2[k];
302 if (factor < 60 * nVectorLength)
305 factor = 1.0f / sqrtf(factor);
306 for (k = 0; k < nVectorLength; k++)
307 vector2[k] *= factor;
310 for (k = 0; k < nVectorLength; k++)
311 value += vector1[k] * vector2[k];
317 if (value > best_value)
334 const int w2 = nWindowSize / 2;
339 const unsigned char *input_left = pInputImageLeft->
pixels;
340 const unsigned char *input_right = pInputImageRight->
pixels;
341 const int nVectorLength = nWindowSize * nWindowSize;
343 int *vector1 =
new int[nVectorLength];
344 int *vector2 =
new int[nVectorLength];
346 const int offset = (y -
w2) * width + (x - w2);
347 const int diff = width - nWindowSize;
348 const Vec2d camera_point = { float(x), float(y) };
352 for (
int yy = 0, offset2 = offset, offset3 = 0; yy < nWindowSize; yy++, offset2 += diff)
353 for (
int x = 0; x < nWindowSize; x++, offset2++, offset3++)
355 vector1[offset3] = input_left[offset2];
356 mean1 += vector1[offset3];
358 mean1 /= nVectorLength;
361 float best_value = FLT_MAX;
362 int d, best_d = -100004;
364 const int max_d = d2 < x ? d2 :
x;
370 for (d = d1; d <= max_d; d++)
372 const int xx = x - d;
373 const int yy = int(m * xx + c + 0.5f);
375 if (xx < w2 || xx >= width - w2 || yy < w2 || yy >= height - w2)
378 const int offset_right = (yy -
w2) * width + (xx - w2);
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++)
385 vector2[offset3] = input_right[offset2];
386 mean2 += vector2[offset3];
389 mean2 /= nVectorLength;
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);
400 if (value < best_value)
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
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.
bool m_bOwnStereoCalibrationObject
Data structure for the representation of a 3D vector.
Data structure for the representation of 8-bit grayscale images and 24-bit RGB (or HSV) color images ...
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.
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...
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
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.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
~CStereoMatcher()
The destructor.
GLenum GLsizei GLsizei height
int GetDisparityEstimate(const float z)
Computes a disparity estimated for a given z-distance.
ImageType type
The type of the image.
bool LoadCameraParameters(const char *pCameraParameterFileName)
Initializes the internally used camera model, given a file path to a camera parameter file...
CStereoMatcher()
The default constructor (and only constructor).
Data structure for the representation of a 2D vector.
void CalculateEpipolarLineInRightImage(const Vec2d &pointInLeftImage, Vec3d &l)
Given an image point in the left image, computes the corresponding epipolar line in the right image...
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.