70 L2 = L3 = L4 = L5 = L7 = L8 = L9 = L10 = L11 = 0;
84 m_pPairElements = pPairElements;
85 m_nPairElements = nPairElements;
91 CalculateDLT(resultCalibration,
true);
92 ExtractFromDLT(resultCalibration);
96 for (
int i = 0; i < nIterations; i++)
98 CalculateDLT(resultCalibration, i == 0);
99 ExtractFromDLT(resultCalibration);
102 CalculateRadialLensDistortion(resultCalibration);
104 CalculateRadialAndTangentialLensDistortion(resultCalibration);
114 const float x = worldPoint.
x;
115 const float y = worldPoint.
y;
116 const float z = worldPoint.
z;
118 imagePoint.
x = (L1 * x + L2 * y + L3 * z + L4) / (L9 * x + L10 * y + L11 * z + 1);
119 imagePoint.
y = (L5 * x + L6 * y + L7 * z + L8) / (L9 * x + L10 * y + L11 * z + 1);
128 for (
int i = 0; i < m_nPairElements; i++)
133 const float distance =
Math2d::Distance(m_pPairElements[i].imagePoint, imagePoint);
141 return error / m_nPairElements;
145 float CDLTCalibration::CheckDLT()
149 for (
int i = 0; i < m_nPairElements; i++)
157 return error / m_nPairElements;
161 void CDLTCalibration::CalculateRadialLensDistortion(
CCalibration &calibration)
171 for (
int i = 0; i < m_nPairElements; i++)
173 const int ii = 2 * i;
175 Vec2d undistorted_point;
178 const float u = undistorted_point.
x;
179 const float v = undistorted_point.
y;
183 const float x = (u - cx) / fx;
184 const float y = (v - cy) / fy;
186 const float rr = x * x + y * y;
188 A(0, ii) = (u - cx) * rr;
189 A(1, ii) = (u - cx) * rr * rr;
190 A(0, ii + 1) = (v - cy) * rr;
191 A(1, ii + 1) = (v - cy) * rr * rr;
200 calibration.
SetDistortion((
float) result[0], (
float) result[1], 0, 0);
204 void CDLTCalibration::CalculateRadialAndTangentialLensDistortion(
CCalibration &calibration)
214 for (
int i = 0; i < m_nPairElements; i++)
216 const int ii = 2 * i;
218 Vec2d undistorted_point;
221 const float u = undistorted_point.
x;
222 const float v = undistorted_point.
y;
226 const float x = (u - cx) / fx;
227 const float y = (v - cy) / fy;
229 const float rr = x * x + y * y;
231 A(0, ii) = (u - cx) * rr;
232 A(1, ii) = (u - cx) * rr * rr;
233 A(2, ii) = 2 * x * y * fx;
234 A(3, ii) = (rr + 2 * x * x) * fx;
235 A(0, ii + 1) = (v - cy) * rr;
236 A(1, ii + 1) = (v - cy) * rr * rr;
237 A(2, ii + 1) = (rr + 2 * y * y) * fy;
238 A(3, ii + 1) = 2 * x * y * fy;
247 calibration.
SetDistortion((
float) result[0], (
float) result[1], (
float) result[2], (
float) result[3]);
251 void CDLTCalibration::ExtractFromDLT(
CCalibration &calibration)
253 const float LL = L9 * L9 + L10 * L10 + L11 * L11;
254 const float L = sqrt(LL);
255 const float atc = L1 * L9 + L2 * L10 + L3 * L11;
256 const float btc = L5 * L9 + L6 * L10 + L7 * L11;
257 const float atb = L1 * L5 * L2 * L6 + L3 * L7;
258 const float ata = L1 * L1 + L2 * L2 + L3 * L3;
259 const float btb = L5 * L5 + L6 * L6 + L7 * L7;
261 const float cx = atc / LL;
262 const float cy = btc / LL;
263 const float fx = sqrt(ata / LL - cx * cx);
264 const float fy = sqrt(btb / LL - cy * cy);
266 const float r31 = L9 / L;
267 const float r32 = L10 / L;
268 const float r33 = L11 / L;
269 const float r11 = (L1 / L - cx * r31) / fx;
270 const float r12 = (L2 / L - cx * r32) / fx;
271 const float r13 = (L3 / L - cx * r33) / fx;
272 const float r21 = (L5 / L - cy * r31) / fy;
273 const float r22 = (L6 / L - cy * r32) / fy;
274 const float r23 = (L7 / L - cy * r33) / fy;
276 const Mat3d rotation = { r11, r12, r13, r21, r22, r23, r31, r32, r33 };
279 Mat3d abc = { L1, L2, L3, L5, L6, L7, L9, L10, L11 };
282 const Vec3d x = { L4, L8, 1 };
292 void CDLTCalibration::CalculateDLT(
const CCalibration &calibration,
bool bFirstCall)
297 for (
int i = 0; i < m_nPairElements; i++)
308 const Vec2d distortedPoint = { u, v };
309 Vec2d undistortedPoint;
313 u = undistortedPoint.
x;
314 v = undistortedPoint.
y;
317 const int ii = 2 * i;
339 A(8, ii + 1) = -v * x;
340 A(9, ii + 1) = -v * y;
341 A(10, ii + 1) = -v * z;
350 L1 = (float) result[0];
351 L2 = (float) result[1];
352 L3 = (float) result[2];
353 L4 = (float) result[3];
354 L5 = (float) result[4];
355 L6 = (float) result[5];
356 L7 = (float) result[6];
357 L8 = (float) result[7];
358 L9 = (float) result[8];
359 L10 = (float) result[9];
360 L11 = (float) result[10];
float CheckCalibration(const CCalibration &calibration)
void Invert(const Mat3d &matrix, Mat3d &result)
Data structure for the representation of a 2D vector.
bool SolveLinearLeastSquaresSimple(const CFloatMatrix *A, const CFloatVector *b, CFloatVector *x)
void UndistortImageCoordinates(const Vec2d &distortedImagePoint, Vec2d &undistortedImagePoint) const
Transforms 2D distorted image coordinates to 2D undistorted image coordinates.
void WorldToImageCoordinates(const Vec3d &worldPoint, Vec2d &imagePoint, bool bUseDistortionParameters=true) const
Transforms 3D world coordinates to 2D image coordinates.
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Data structure for the representation of a matrix of values of the data type float.
Data structure for the representation of a 3D vector.
void MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
void SetDistortion(float d1, float d2, float d3, float d4)
Sets the distortion parameters of the distortion model.
Camera model parameters and functions for a single camera.
void GetImageCoordinatesDLT(const Vec3d &worldPoint, Vec2d &imagePoint)
float Calibrate(const PairElement *pPairElements, int nPairElements, CCalibration &resultCalibration, DistortionType eCalculateDistortionParameters=eNoDistortion, int nIterations=1000)
void SetIntrinsicBase(float cx, float cy, float fx, float fy)
Sets the principal point and the focal lengths.
void SetRotation(const Mat3d &R)
Sets the extrinsic parameter R (rotation matrix).
Data structure for the representation of a vector of values of the data type float.
float Distance(const Vec2d &vector1, const Vec2d &vector2)
Data structure for the representation of a 3x3 matrix.
void SetTranslation(const Vec3d &t)
Sets the extrinsic parameter t (translation vector).