96 for (
int i = 0; i < nIterations; i++)
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);
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);
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]);
254 const float L = sqrt(LL);
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 };
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];
void CalculateDLT(const CCalibration &calibration, bool bFirstCall)
void SetDistortion(float d1, float d2, float d3, float d4)
Sets the distortion parameters of the distortion model.
void UndistortImageCoordinates(const Vec2d &distortedImagePoint, Vec2d &undistortedImagePoint) const
Transforms 2D distorted image coordinates to 2D undistorted image coordinates.
Data structure for the representation of a 3D vector.
void CalculateRadialLensDistortion(CCalibration &calibration)
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
float Distance(const Vec2d &vector1, const Vec2d &vector2)
const PairElement * m_pPairElements
float CheckCalibration(const CCalibration &calibration)
Data structure for the representation of a matrix of values of the data type float.
void SetIntrinsicBase(float cx, float cy, float fx, float fy)
Sets the principal point and the focal lengths.
void Invert(const Mat3d &matrix, Mat3d &result)
void MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
void ExtractFromDLT(CCalibration &calibration)
void CalculateRadialAndTangentialLensDistortion(CCalibration &calibration)
void GetImageCoordinatesDLT(const Vec3d &worldPoint, Vec2d &imagePoint)
Data structure for the representation of a vector of values of the data type float.
bool SolveLinearLeastSquaresSimple(const CFloatMatrix *A, const CFloatVector *b, CFloatVector *x)
float Calibrate(const PairElement *pPairElements, int nPairElements, CCalibration &resultCalibration, DistortionType eCalculateDistortionParameters=eNoDistortion, int nIterations=1000)
Data structure for the representation of a 2D vector.
void SetTranslation(const Vec3d &t)
Sets the extrinsic parameter t (translation vector).
void SetRotation(const Mat3d &R)
Sets the extrinsic parameter R (rotation matrix).
Data structure for the representation of a 3x3 matrix.
Camera model parameters and functions for a single camera.
void WorldToImageCoordinates(const Vec3d &worldPoint, Vec2d &imagePoint, bool bUseDistortionParameters=true) const
Transforms 3D world coordinates to 2D image coordinates.