StereoCalibrationCV.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 
37 // ****************************************************************************
38 // Includes
39 // ****************************************************************************
40 
41 #include <new> // for explicitly using correct new/delete operators on VC DSPs
42 
43 #include "StereoCalibrationCV.h"
44 #include "StereoCalibration.h"
45 #include "Calibration.h"
46 #include "Math/Math3d.h"
47 
48 #include <stdio.h>
49 
50 
51 
52 static void FillMatrix(float *pTargetMatrix, const Mat3d &sourceMatrix)
53 {
54  pTargetMatrix[0] = sourceMatrix.r1;
55  pTargetMatrix[1] = sourceMatrix.r2;
56  pTargetMatrix[2] = sourceMatrix.r3;
57  pTargetMatrix[3] = sourceMatrix.r4;
58  pTargetMatrix[4] = sourceMatrix.r5;
59  pTargetMatrix[5] = sourceMatrix.r6;
60  pTargetMatrix[6] = sourceMatrix.r7;
61  pTargetMatrix[7] = sourceMatrix.r8;
62  pTargetMatrix[8] = sourceMatrix.r9;
63 }
64 
65 static void FillMatrix(double *pTargetMatrix, const Mat3d &sourceMatrix)
66 {
67  pTargetMatrix[0] = sourceMatrix.r1;
68  pTargetMatrix[1] = sourceMatrix.r2;
69  pTargetMatrix[2] = sourceMatrix.r3;
70  pTargetMatrix[3] = sourceMatrix.r4;
71  pTargetMatrix[4] = sourceMatrix.r5;
72  pTargetMatrix[5] = sourceMatrix.r6;
73  pTargetMatrix[6] = sourceMatrix.r7;
74  pTargetMatrix[7] = sourceMatrix.r8;
75  pTargetMatrix[8] = sourceMatrix.r9;
76 }
77 
78 static void FillVector(float *pTargetVector, const Vec3d &sourceVector)
79 {
80  pTargetVector[0] = (float) sourceVector.x;
81  pTargetVector[1] = (float) sourceVector.y;
82  pTargetVector[2] = (float) sourceVector.z;
83 }
84 
85 static void FillCalibrationMatrix(float *pTargetMatrix, const CCalibration::CCameraParameters &cameraParameters)
86 {
87  pTargetMatrix[0] = (float) cameraParameters.focalLength.x;
88  pTargetMatrix[1] = 0;
89  pTargetMatrix[2] = (float) cameraParameters.principalPoint.x;
90  pTargetMatrix[3] = 0;
91  pTargetMatrix[4] = (float) cameraParameters.focalLength.y;
92  pTargetMatrix[5] = (float) cameraParameters.principalPoint.y;
93  pTargetMatrix[6] = 0;
94  pTargetMatrix[7] = 0;
95  pTargetMatrix[8] = 1;
96 }
97 
98 
99 void CalculateRectificationHomographies(CStereoCalibration *pStereoCalibration)
100 {
101  CvStereoCamera stereoParams;
102  CvCamera leftCamera, rightCamera;
103 
104  stereoParams.camera[0] = &leftCamera;
105  stereoParams.camera[1] = &rightCamera;
106 
107  stereoParams.camera[0]->imgSize[0] = (float) pStereoCalibration->width;
108  stereoParams.camera[0]->imgSize[1] = (float) pStereoCalibration->height;
109  stereoParams.camera[1]->imgSize[0] = (float) pStereoCalibration->width;
110  stereoParams.camera[1]->imgSize[1] = (float) pStereoCalibration->height;
111 
112  const CCalibration::CCameraParameters &leftCameraParameters = pStereoCalibration->GetLeftCalibration()->GetCameraParameters();
113  const CCalibration::CCameraParameters &rightCameraParameters = pStereoCalibration->GetRightCalibration()->GetCameraParameters();
114 
115  FillMatrix(stereoParams.rotMatrix, pStereoCalibration->GetRightCalibration()->m_rotation_inverse);
116  FillVector(stereoParams.transVector, pStereoCalibration->GetRightCalibration()->m_translation_inverse);
117 
118  int i;
119 
120  leftCamera.imgSize[0] = (float) leftCameraParameters.width;
121  leftCamera.imgSize[1] = (float) leftCameraParameters.height;
122  FillCalibrationMatrix(leftCamera.matrix, leftCameraParameters);
123  for (i = 0; i < 4; i++) leftCamera.distortion[i] = (float) leftCameraParameters.distortion[i];
124  FillMatrix(leftCamera.rotMatr, leftCameraParameters.rotation);
125  FillVector(leftCamera.transVect, leftCameraParameters.translation);
126 
127  rightCamera.imgSize[0] = (float) rightCameraParameters.width;
128  rightCamera.imgSize[1] = (float) rightCameraParameters.height;
129  FillCalibrationMatrix(rightCamera.matrix, rightCameraParameters);
130  for (i = 0; i < 4; i++) rightCamera.distortion[i] = (float) rightCameraParameters.distortion[i];
131  FillMatrix(rightCamera.rotMatr, rightCameraParameters.rotation);
132  FillVector(rightCamera.transVect, rightCameraParameters.translation);
133 
134  icvComputeRestStereoParams(&stereoParams);
135 
136  Math3d::SetMat(pStereoCalibration->rectificationHomographyLeft,
137  float(stereoParams.coeffs[0][0][0]),
138  float(stereoParams.coeffs[0][0][1]),
139  float(stereoParams.coeffs[0][0][2]),
140  float(stereoParams.coeffs[0][1][0]),
141  float(stereoParams.coeffs[0][1][1]),
142  float(stereoParams.coeffs[0][1][2]),
143  float(stereoParams.coeffs[0][2][0]),
144  float(stereoParams.coeffs[0][2][1]),
145  float(stereoParams.coeffs[0][2][2])
146  );
147 
148  Math3d::SetMat(pStereoCalibration->rectificationHomographyRight,
149  float(stereoParams.coeffs[1][0][0]),
150  float(stereoParams.coeffs[1][0][1]),
151  float(stereoParams.coeffs[1][0][2]),
152  float(stereoParams.coeffs[1][1][0]),
153  float(stereoParams.coeffs[1][1][1]),
154  float(stereoParams.coeffs[1][1][2]),
155  float(stereoParams.coeffs[1][2][0]),
156  float(stereoParams.coeffs[1][2][1]),
157  float(stereoParams.coeffs[1][2][2])
158  );
159 }
160 
161 /*void StereoCalibrationCV::CalculateRectificationHomographiesExperimental(CStereoCalibration *pStereoCalibration)
162 {
163  // algorithm by Fusiello et al. from 2000:
164  // "A compact algorithm for rectification of stereo pairs"
165  const CCalibration::CCameraParameters &left = pStereoCalibration->GetLeftCalibration()->GetCameraParameters();
166  const CCalibration::CCameraParameters &right = pStereoCalibration->GetRightCalibration()->GetCameraParameters();
167 
168  const Mat3d &R1 = pStereoCalibration->GetLeftCalibration()->m_rotation_inverse;//left.rotation;
169  const Mat3d &R2 = pStereoCalibration->GetRightCalibration()->m_rotation_inverse;//right.rotation;
170 
171  const Vec3d &t1 = pStereoCalibration->GetLeftCalibration()->m_translation_inverse;//left.translation;
172  const Vec3d &t2 = pStereoCalibration->GetRightCalibration()->m_translation_inverse;//right.translation;
173 
174  const Mat3d A1 = { -left.focalLength.x, 0, left.principalPoint.x, 0, -left.focalLength.y, left.principalPoint.y, 0, 0, 1 };
175  const Mat3d A2 = { -right.focalLength.x, 0, right.principalPoint.x, 0, -right.focalLength.y, right.principalPoint.y, 0, 0, 1 };
176 
177  printf("\nA1 = %f %f %f\n%f %f %f\n%f %f %f\n\n", A1.r1, A1.r2, A1.r3, A1.r4, A1.r5, A1.r6, A1.r7, A1.r8, A1.r9);
178  printf("R1 = %f %f %f\n%f %f %f\n%f %f %f\n\n", R1.r1, R1.r2, R1.r3, R1.r4, R1.r5, R1.r6, R1.r7, R1.r8, R1.r9);
179  printf("t1 = %f %f %f\n\n\n", t1.x, t1.y, t1.z);
180 
181  printf("A2 = %f %f %f\n%f %f %f\n%f %f %f\n\n", A2.r1, A2.r2, A2.r3, A2.r4, A2.r5, A2.r6, A2.r7, A2.r8, A2.r9);
182  printf("R2 = %f %f %f\n%f %f %f\n%f %f %f\n\n", R2.r1, R2.r2, R2.r3, R2.r4, R2.r5, R2.r6, R2.r7, R2.r8, R2.r9);
183  printf("t2 = %f %f %f\n\n\n", t2.x, t2.y, t2.z);
184 
185  const Vec3d &c1 = pStereoCalibration->GetLeftCalibration()->m_translation_inverse;
186  const Vec3d &c2 = pStereoCalibration->GetRightCalibration()->m_translation_inverse;
187 
188  Vec3d v1, v2, v3, k = { R1.r7, R1.r8, R1.r9 };
189 
190  Math3d::SubtractVecVec(c1, c2, v1);
191  Math3d::CrossProduct(k, v1, v2);
192  Math3d::CrossProduct(v1, v2, v3);
193 
194  Math3d::NormalizeVec(v1);
195  Math3d::NormalizeVec(v2);
196  Math3d::NormalizeVec(v3);
197 
198  Mat3d R = { v1.x, v1.y, v1.z, v2.x, v2.y, v2.z, v3.x, v3.y, v3.z };
199  Mat3d A =
200  {
201  0.5f * (left.focalLength.x + right.focalLength.x), 0, 0.5f * (left.principalPoint.x + right.principalPoint.x),
202  0, 0.5f * (left.focalLength.y + right.focalLength.y), 0.5f * (left.principalPoint.y + right.principalPoint.y),
203  0, 0, 1
204  };
205 
206  Mat3d T1, T2, M;
207 
208  Math3d::MulMatMat(A, R, T1);
209  Math3d::MulMatMat(A1, R1, M);
210  Math3d::Invert(M, M);
211  Math3d::MulMatMat(T1, M, T1);
212 
213  Math3d::MulMatMat(A, R, T2);
214  Math3d::MulMatMat(A2, R2, M);
215  Math3d::Invert(M, M);
216  Math3d::MulMatMat(T2, M, T2);
217 
218  Math3d::SetMat(pStereoCalibration->rectificationHomographyLeft, T1);
219  Math3d::SetMat(pStereoCalibration->rectificationHomographyRight, T2);
220 }*/
221 
Mat3d::r4
float r4
Definition: Math3d.h:95
CCalibration::CCameraParameters
Struct containing all parameters of the camera model.
Definition: Calibration.h:133
FillCalibrationMatrix
static void FillCalibrationMatrix(float *pTargetMatrix, const CCalibration::CCameraParameters &cameraParameters)
Definition: StereoCalibrationCV.cpp:85
CCalibration::m_rotation_inverse
Mat3d m_rotation_inverse
Rotation matrix of the inverted extrinsic transformation.
Definition: Calibration.h:443
CStereoCalibration::rectificationHomographyRight
Mat3d rectificationHomographyRight
The homography for the rectification mapping of the right image.
Definition: StereoCalibration.h:370
FillMatrix
static void FillMatrix(float *pTargetMatrix, const Mat3d &sourceMatrix)
Definition: StereoCalibrationCV.cpp:52
Mat3d::r7
float r7
Definition: Math3d.h:95
Vec2d::y
float y
Definition: Math2d.h:84
Mat3d::r1
float r1
Definition: Math3d.h:95
Mat3d::r2
float r2
Definition: Math3d.h:95
Mat3d::r3
float r3
Definition: Math3d.h:95
Vec3d
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
Vec3d::x
float x
Definition: Math3d.h:75
Vec3d::z
float z
Definition: Math3d.h:75
CStereoCalibration::GetLeftCalibration
const CCalibration * GetLeftCalibration() const
Access to the instance of CCalibration for the camera model of the left camera.
Definition: StereoCalibration.h:211
CCalibration::GetCameraParameters
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Definition: Calibration.h:268
Vec2d::x
float x
Definition: Math2d.h:84
CStereoCalibration::height
int height
The height of the images of the stereo camera system in pixels.
Definition: StereoCalibration.h:348
Vec3d::y
float y
Definition: Math3d.h:75
Mat3d::r6
float r6
Definition: Math3d.h:95
CStereoCalibration::width
int width
The width of the images of the stereo camera system in pixels.
Definition: StereoCalibration.h:346
CStereoCalibration
Camera model and functions for a stereo camera system.
Definition: StereoCalibration.h:87
Mat3d::r8
float r8
Definition: Math3d.h:95
Mat3d::r9
float r9
Definition: Math3d.h:95
CCalibration::m_translation_inverse
Vec3d m_translation_inverse
Translation vector of the inverted extrinsic transformation.
Definition: Calibration.h:454
CStereoCalibration::rectificationHomographyLeft
Mat3d rectificationHomographyLeft
The homography for the rectification mapping of the right image.
Definition: StereoCalibration.h:359
Mat3d::r5
float r5
Definition: Math3d.h:95
CalculateRectificationHomographies
void CalculateRectificationHomographies(CStereoCalibration *pStereoCalibration)
Definition: StereoCalibrationCV.cpp:99
Math3d::SetMat
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
Mat3d
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
CStereoCalibration::GetRightCalibration
const CCalibration * GetRightCalibration() const
Access to the instance of CCalibration for the camera model of the right camera.
Definition: StereoCalibration.h:214
FillVector
static void FillVector(float *pTargetVector, const Vec3d &sourceVector)
Definition: StereoCalibrationCV.cpp:78

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