From e85bacff7b676a7789bea6501e694cac299b21be Mon Sep 17 00:00:00 2001
From: Pierre-Emmanuel Viel
Date: Thu, 26 Dec 2013 19:44:23 +0100
Subject: [PATCH 001/189] Avoid obtaining several identical dimensions between
two LSH sub-vectors by choosing orthogonal sub-vectors.
---
modules/flann/include/opencv2/flann/lsh_table.h | 16 ++++++++++++----
1 file changed, 12 insertions(+), 4 deletions(-)
diff --git a/modules/flann/include/opencv2/flann/lsh_table.h b/modules/flann/include/opencv2/flann/lsh_table.h
index b0f3223851..6b8d614f23 100644
--- a/modules/flann/include/opencv2/flann/lsh_table.h
+++ b/modules/flann/include/opencv2/flann/lsh_table.h
@@ -348,13 +348,21 @@ inline LshTable::LshTable(unsigned int feature_size, unsigned int
mask_ = std::vector((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0);
// A bit brutal but fast to code
- std::vector indices(feature_size * CHAR_BIT);
- for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
- std::random_shuffle(indices.begin(), indices.end());
+ static std::vector indices(feature_size * CHAR_BIT);
+
+ //Ensure the Nth bit will be selected only once among the different LshTables
+ //to avoid having two different tables with signatures sharing many dimensions/many bits
+ if( (indices.size() == feature_size * CHAR_BIT) || (indices.size() < key_size_) )
+ {
+ indices.resize( feature_size * CHAR_BIT );
+ for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
+ std::random_shuffle(indices.begin(), indices.end());
+ }
// Generate a random set of order of subsignature_size_ bits
for (unsigned int i = 0; i < key_size_; ++i) {
- size_t index = indices[i];
+ size_t index = indices[0];
+ indices.erase( indices.begin() );
// Set that bit in the mask
size_t divisor = CHAR_BIT * sizeof(size_t);
From 8e93c19de311a76704028b0195abfe04cfad900d Mon Sep 17 00:00:00 2001
From: Pierre-Emmanuel Viel
Date: Thu, 26 Dec 2013 23:04:54 +0100
Subject: [PATCH 002/189] Fix a heap issue with static on Windows
---
.../flann/include/opencv2/flann/lsh_table.h | 20 ++++++++++++-------
1 file changed, 13 insertions(+), 7 deletions(-)
diff --git a/modules/flann/include/opencv2/flann/lsh_table.h b/modules/flann/include/opencv2/flann/lsh_table.h
index 6b8d614f23..18fb139c91 100644
--- a/modules/flann/include/opencv2/flann/lsh_table.h
+++ b/modules/flann/include/opencv2/flann/lsh_table.h
@@ -348,21 +348,27 @@ inline LshTable::LshTable(unsigned int feature_size, unsigned int
mask_ = std::vector((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0);
// A bit brutal but fast to code
- static std::vector indices(feature_size * CHAR_BIT);
+ static std::vector* indices = NULL;
//Ensure the Nth bit will be selected only once among the different LshTables
//to avoid having two different tables with signatures sharing many dimensions/many bits
- if( (indices.size() == feature_size * CHAR_BIT) || (indices.size() < key_size_) )
+ if( indices == NULL )
{
- indices.resize( feature_size * CHAR_BIT );
- for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
- std::random_shuffle(indices.begin(), indices.end());
+ indices = new std::vector( feature_size * CHAR_BIT );
+ }
+ else if( indices->size() < key_size_ )
+ {
+ indices->resize( feature_size * CHAR_BIT );
+ for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) {
+ (*indices)[i] = i;
+ }
+ std::random_shuffle(indices->begin(), indices->end());
}
// Generate a random set of order of subsignature_size_ bits
for (unsigned int i = 0; i < key_size_; ++i) {
- size_t index = indices[0];
- indices.erase( indices.begin() );
+ size_t index = (*indices)[0];
+ indices->erase( indices->begin() );
// Set that bit in the mask
size_t divisor = CHAR_BIT * sizeof(size_t);
From 9c7e0bfd33cb65a57b06d1bf9a81e663fda01d22 Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Fri, 25 Apr 2014 14:49:36 +0400
Subject: [PATCH 003/189] Added fisheye camera model
---
.../include/opencv2/calib3d/calib3d.hpp | 127 ++
modules/calib3d/src/fisheye.cpp | 1217 +++++++++++++++++
modules/calib3d/test/test_fisheye.cpp | 423 ++++++
3 files changed, 1767 insertions(+)
create mode 100644 modules/calib3d/src/fisheye.cpp
create mode 100644 modules/calib3d/test/test_fisheye.cpp
diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
index f213a114f4..2675ad402a 100644
--- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
@@ -45,6 +45,7 @@
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
+#include "opencv2/core/affine.hpp"
#ifdef __cplusplus
extern "C" {
@@ -744,6 +745,132 @@ CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst,
OutputArray out, OutputArray inliers,
double ransacThreshold=3, double confidence=0.99);
+
+class Fisheye
+{
+public:
+
+ //Definitions:
+ // Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
+ // The coordinate vector of P in the camera reference frame is: Xc = R*X + T
+ // where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
+ // call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
+ // The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
+ // call r^2 = a^2 + b^2,
+ // call theta = atan(r),
+ //
+ // Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8)
+ //
+ // The distorted point coordinates are: xd = [xx;yy] where:
+ //
+ // xx = (theta_d / r) * x
+ // yy = (theta_d / r) * y
+ //
+ // Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
+ //
+ // xxp = f(1)*(xx + alpha*yy) + c(1)
+ // yyp = f(2)*yy + c(2)
+
+ enum{
+ CALIB_USE_INTRINSIC_GUESS = 1,
+ CALIB_RECOMPUTE_EXTRINSIC = 2,
+ CALIB_CHECK_COND = 4,
+ CALIB_FIX_SKEW = 8,
+ CALIB_FIX_K1 = 16,
+ CALIB_FIX_K2 = 32,
+ CALIB_FIX_K3 = 64,
+ CALIB_FIX_K4 = 128
+ };
+
+ //! projects 3D points using fisheye model
+ static void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
+ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
+
+ //! projects points using fisheye model
+ static void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec,
+ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
+
+ //! distorts 2D points using fisheye model
+ static void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0);
+
+ //! undistorts 2D points using fisheye model
+ static void undistortPoints(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray());
+
+ //! computing undistortion and rectification maps for image transform by cv::remap()
+ //! If D is empty zero distortion is used, if R or P is empty identity matrixes are used
+ static void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
+ const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);
+
+ //! undistorts image, optinally chanes resolution and camera matrix. If Knew zero identity matrix is used
+ static void undistortImage(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size());
+
+ //! estimates new camera matrix for undistortion or rectification
+ static void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
+ OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0);
+
+ //! stereo rectification for fisheye camera model
+ static void stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const cv::Size& imageSize,
+ InputArray rotaion, InputArray tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q,
+ int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, const Size& newImageSize = Size(), Rect* roi1 = 0, Rect* roi2 = 0 );
+
+ //! performs camera calibaration
+ static double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
+ InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0,
+ TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
+
+ //! stereo rectification estimation
+ static void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
+ OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
+ double balance = 0.0, double fov_scale = 1.0);
+};
+
+
+
+namespace internal {
+
+struct IntrinsicParams
+{
+ Vec2d f;
+ Vec2d c;
+ Vec4d k;
+ double alpha;
+ std::vector isEstimate;
+
+ IntrinsicParams();
+ IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
+ IntrinsicParams operator+(const Mat& a);
+ IntrinsicParams& operator =(const Mat& a);
+ void Init(const cv::Vec2d& f, const cv::Vec2d& c, const cv::Vec4d& k = Vec4d(0,0,0,0), const double& alpha = 0);
+};
+
+void projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
+ cv::InputArray _rvec,cv::InputArray _tvec,
+ const IntrinsicParams& param, cv::OutputArray jacobian);
+
+void ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
+ Mat& tvec, Mat& J, const int MaxIter,
+ const IntrinsicParams& param, const double thresh_cond);
+Mat ComputeHomography(Mat m, Mat M);
+
+Mat NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param);
+
+void InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk);
+
+void CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, const int check_cond,
+ const double thresh_cond, InputOutputArray omc, InputOutputArray Tc);
+
+void ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, InputArray omc, InputArray Tc,
+ const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3);
+
+void EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& params, InputArray omc, InputArray Tc,
+ IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms);
+
+}
}
#endif
diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp
new file mode 100644
index 0000000000..0dcc5e70fd
--- /dev/null
+++ b/modules/calib3d/src/fisheye.cpp
@@ -0,0 +1,1217 @@
+#include "opencv2/opencv.hpp"
+#include "opencv2/core/affine.hpp"
+#include "opencv2/core/affine.hpp"
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::projectPoints
+
+namespace cv { namespace
+{
+ struct JacobianRow
+ {
+ Vec2d df, dc;
+ Vec4d dk;
+ Vec3d dom, dT;
+ double dalpha;
+ };
+}}
+
+void cv::Fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
+ InputArray K, InputArray D, double alpha, OutputArray jacobian)
+{
+ projectPoints(objectPoints, imagePoints, affine.rvec(), affine.translation(), K, D, alpha, jacobian);
+}
+
+void cv::Fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec,
+ InputArray _tvec, InputArray _K, InputArray _D, double alpha, OutputArray jacobian)
+{
+ // will support only 3-channel data now for points
+ CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
+ imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
+ size_t n = objectPoints.total();
+
+ CV_Assert(_rvec.total() * _rvec.channels() == 3 && (_rvec.depth() == CV_32F || _rvec.depth() == CV_64F));
+ CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F));
+ CV_Assert(_tvec.getMat().isContinuous() && _rvec.getMat().isContinuous());
+
+ Vec3d om = _rvec.depth() == CV_32F ? (Vec3d)*_rvec.getMat().ptr() : *_rvec.getMat().ptr();
+ Vec3d T = _tvec.depth() == CV_32F ? (Vec3d)*_tvec.getMat().ptr() : *_tvec.getMat().ptr();
+
+ CV_Assert(_K.size() == Size(3,3) && (_K.type() == CV_32F || _K.type() == CV_64F) && _D.type() == _K.type() && _D.total() == 4);
+
+ cv::Vec2d f, c;
+ if (_K.depth() == CV_32F)
+ {
+ Matx33f K = _K.getMat();
+ f = Vec2f(K(0, 0), K(1, 1));
+ c = Vec2f(K(0, 2), K(1, 2));
+ }
+ else
+ {
+ Matx33d K = _K.getMat();
+ f = Vec2d(K(0, 0), K(1, 1));
+ c = Vec2d(K(0, 2), K(1, 2));
+ }
+
+ Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr(): *_D.getMat().ptr();
+
+ JacobianRow *Jn = 0;
+ if (jacobian.needed())
+ {
+ int nvars = 2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T,
+ jacobian.create(2*(int)n, nvars, CV_64F);
+ Jn = jacobian.getMat().ptr(0);
+ }
+
+ Matx33d R;
+ Matx dRdom;
+ Rodrigues(om, R, dRdom);
+ Affine3d aff(om, T);
+
+ const Vec3f* Xf = objectPoints.getMat().ptr();
+ const Vec3d* Xd = objectPoints.getMat().ptr();
+ Vec2f *xpf = imagePoints.getMat().ptr();
+ Vec2d *xpd = imagePoints.getMat().ptr();
+
+ for(size_t i = 0; i < n; ++i)
+ {
+ Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i];
+ Vec3d Y = aff*Xi;
+
+ Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);
+
+ double r2 = x.dot(x);
+ double r = std::sqrt(r2);
+
+ // Angle of the incoming ray:
+ double theta = atan(r);
+
+ double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
+ theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
+
+ double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
+
+ double inv_r = r > 1e-8 ? 1.0/r : 1;
+ double cdist = r > 1e-8 ? theta_d * inv_r : 1;
+
+ Vec2d xd1 = x * cdist;
+ Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
+ Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]);
+
+ if (objectPoints.depth() == CV_32F)
+ xpf[i] = final_point;
+ else
+ xpd[i] = final_point;
+
+ if (jacobian.needed())
+ {
+ //Vec3d Xi = pdepth == CV_32F ? (Vec3d)Xf[i] : Xd[i];
+ //Vec3d Y = aff*Xi;
+ double dYdR[] = { Xi[0], Xi[1], Xi[2], 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, Xi[0], Xi[1], Xi[2], 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, Xi[0], Xi[1], Xi[2] };
+
+ Matx33d dYdom_data = Matx(dYdR) * dRdom.t();
+ const Vec3d *dYdom = (Vec3d*)dYdom_data.val;
+
+ Matx33d dYdT_data = Matx33d::eye();
+ const Vec3d *dYdT = (Vec3d*)dYdT_data.val;
+
+ //Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);
+ Vec3d dxdom[2];
+ dxdom[0] = (1.0/Y[2]) * dYdom[0] - x[0]/Y[2] * dYdom[2];
+ dxdom[1] = (1.0/Y[2]) * dYdom[1] - x[1]/Y[2] * dYdom[2];
+
+ Vec3d dxdT[2];
+ dxdT[0] = (1.0/Y[2]) * dYdT[0] - x[0]/Y[2] * dYdT[2];
+ dxdT[1] = (1.0/Y[2]) * dYdT[1] - x[1]/Y[2] * dYdT[2];
+
+ //double r2 = x.dot(x);
+ Vec3d dr2dom = 2 * x[0] * dxdom[0] + 2 * x[1] * dxdom[1];
+ Vec3d dr2dT = 2 * x[0] * dxdT[0] + 2 * x[1] * dxdT[1];
+
+ //double r = std::sqrt(r2);
+ double drdr2 = r > 1e-8 ? 1.0/(2*r) : 1;
+ Vec3d drdom = drdr2 * dr2dom;
+ Vec3d drdT = drdr2 * dr2dT;
+
+ // Angle of the incoming ray:
+ //double theta = atan(r);
+ double dthetadr = 1.0/(1+r2);
+ Vec3d dthetadom = dthetadr * drdom;
+ Vec3d dthetadT = dthetadr * drdT;
+
+ //double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
+ double dtheta_ddtheta = 1 + 3*k[0]*theta2 + 5*k[1]*theta4 + 7*k[2]*theta6 + 9*k[3]*theta8;
+ Vec3d dtheta_ddom = dtheta_ddtheta * dthetadom;
+ Vec3d dtheta_ddT = dtheta_ddtheta * dthetadT;
+ Vec4d dtheta_ddk = Vec4d(theta3, theta5, theta7, theta9);
+
+ //double inv_r = r > 1e-8 ? 1.0/r : 1;
+ //double cdist = r > 1e-8 ? theta_d / r : 1;
+ Vec3d dcdistdom = inv_r * (dtheta_ddom - cdist*drdom);
+ Vec3d dcdistdT = inv_r * (dtheta_ddT - cdist*drdT);
+ Vec4d dcdistdk = inv_r * dtheta_ddk;
+
+ //Vec2d xd1 = x * cdist;
+ Vec4d dxd1dk[2];
+ Vec3d dxd1dom[2], dxd1dT[2];
+ dxd1dom[0] = x[0] * dcdistdom + cdist * dxdom[0];
+ dxd1dom[1] = x[1] * dcdistdom + cdist * dxdom[1];
+ dxd1dT[0] = x[0] * dcdistdT + cdist * dxdT[0];
+ dxd1dT[1] = x[1] * dcdistdT + cdist * dxdT[1];
+ dxd1dk[0] = x[0] * dcdistdk;
+ dxd1dk[1] = x[1] * dcdistdk;
+
+ //Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
+ Vec4d dxd3dk[2];
+ Vec3d dxd3dom[2], dxd3dT[2];
+ dxd3dom[0] = dxd1dom[0] + alpha * dxd1dom[1];
+ dxd3dom[1] = dxd1dom[1];
+ dxd3dT[0] = dxd1dT[0] + alpha * dxd1dT[1];
+ dxd3dT[1] = dxd1dT[1];
+ dxd3dk[0] = dxd1dk[0] + alpha * dxd1dk[1];
+ dxd3dk[1] = dxd1dk[1];
+
+ Vec2d dxd3dalpha(xd1[1], 0);
+
+ //final jacobian
+ Jn[0].dom = f[0] * dxd3dom[0];
+ Jn[1].dom = f[1] * dxd3dom[1];
+
+ Jn[0].dT = f[0] * dxd3dT[0];
+ Jn[1].dT = f[1] * dxd3dT[1];
+
+ Jn[0].dk = f[0] * dxd3dk[0];
+ Jn[1].dk = f[1] * dxd3dk[1];
+
+ Jn[0].dalpha = f[0] * dxd3dalpha[0];
+ Jn[1].dalpha = 0; //f[1] * dxd3dalpha[1];
+
+ Jn[0].df = Vec2d(xd3[0], 0);
+ Jn[1].df = Vec2d(0, xd3[1]);
+
+ Jn[0].dc = Vec2d(1, 0);
+ Jn[1].dc = Vec2d(0, 1);
+
+ //step to jacobian rows for next point
+ Jn += 2;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::distortPoints
+
+void cv::Fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha)
+{
+ // will support only 2-channel data now for points
+ CV_Assert(undistorted.type() == CV_32FC2 || undistorted.type() == CV_64FC2);
+ distorted.create(undistorted.size(), undistorted.type());
+ size_t n = undistorted.total();
+
+ CV_Assert(K.size() == Size(3,3) && (K.type() == CV_32F || K.type() == CV_64F) && D.total() == 4);
+
+ cv::Vec2d f, c;
+ if (K.depth() == CV_32F)
+ {
+ Matx33f camMat = K.getMat();
+ f = Vec2f(camMat(0, 0), camMat(1, 1));
+ c = Vec2f(camMat(0, 2), camMat(1, 2));
+ }
+ else
+ {
+ Matx33d camMat = K.getMat();
+ f = Vec2d(camMat(0, 0), camMat(1, 1));
+ c = Vec2d(camMat(0 ,2), camMat(1, 2));
+ }
+
+ Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr();
+
+ const Vec2f* Xf = undistorted.getMat().ptr();
+ const Vec2d* Xd = undistorted.getMat().ptr();
+ Vec2f *xpf = distorted.getMat().ptr();
+ Vec2d *xpd = distorted.getMat().ptr();
+
+ for(size_t i = 0; i < n; ++i)
+ {
+ Vec2d x = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i];
+
+ double r2 = x.dot(x);
+ double r = std::sqrt(r2);
+
+ // Angle of the incoming ray:
+ double theta = atan(r);
+
+ double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
+ theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
+
+ double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
+
+ double inv_r = r > 1e-8 ? 1.0/r : 1;
+ double cdist = r > 1e-8 ? theta_d * inv_r : 1;
+
+ Vec2d xd1 = x * cdist;
+ Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
+ Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]);
+
+ if (undistorted.depth() == CV_32F)
+ xpf[i] = final_point;
+ else
+ xpd[i] = final_point;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::undistortPoints
+
+void cv::Fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray _K, InputArray _D, InputArray _R, InputArray _P)
+{
+ // will support only 2-channel data now for points
+ CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2);
+ undistorted.create(distorted.size(), distorted.type());
+
+ CV_Assert(_P.empty() || _P.size() == Size(3, 3) || _P.size() == Size(4, 3));
+ CV_Assert(_R.empty() || _R.size() == Size(3, 3) || _R.total() * _R.channels() == 3);
+ CV_Assert(_D.total() == 4 && _K.size() == Size(3, 3) && (_K.depth() == CV_32F || _K.depth() == CV_64F));
+
+ cv::Vec2d f, c;
+ if (_K.depth() == CV_32F)
+ {
+ Matx33f camMat = _K.getMat();
+ f = Vec2f(camMat(0, 0), camMat(1, 1));
+ c = Vec2f(camMat(0, 2), camMat(1, 2));
+ }
+ else
+ {
+ Matx33d camMat = _K.getMat();
+ f = Vec2d(camMat(0, 0), camMat(1, 1));
+ c = Vec2d(camMat(0, 2), camMat(1, 2));
+ }
+
+ Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr(): *_D.getMat().ptr();
+
+ cv::Matx33d RR = cv::Matx33d::eye();
+ if (!_R.empty() && _R.total() * _R.channels() == 3)
+ {
+ cv::Vec3d rvec;
+ _R.getMat().convertTo(rvec, CV_64F);
+ RR = cv::Affine3d(rvec).rotation();
+ }
+ else if (!_R.empty() && _R.size() == Size(3, 3))
+ _R.getMat().convertTo(RR, CV_64F);
+
+ if(!_P.empty())
+ {
+ cv::Matx33d P;
+ _P.getMat().colRange(0, 3).convertTo(P, CV_64F);
+ RR = P * RR;
+ }
+
+ // start undistorting
+ const cv::Vec2f* srcf = distorted.getMat().ptr();
+ const cv::Vec2d* srcd = distorted.getMat().ptr();
+ cv::Vec2f* dstf = undistorted.getMat().ptr();
+ cv::Vec2d* dstd = undistorted.getMat().ptr();
+
+ size_t n = distorted.total();
+ int sdepth = distorted.depth();
+
+ for(size_t i = 0; i < n; i++ )
+ {
+ Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point
+ Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point
+
+ double scale = 1.0;
+
+ double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]);
+ if (theta_d > 1e-8)
+ {
+ // compensate distortion iteratively
+ double theta = theta_d;
+ for(int j = 0; j < 10; j++ )
+ {
+ double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2;
+ theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8);
+ }
+
+ scale = std::tan(theta) / theta_d;
+ }
+
+ Vec2d pu = pw * scale; //undistorted point
+
+ // reproject
+ Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix
+ Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final
+
+ if( sdepth == CV_32F )
+ dstf[i] = fi;
+ else
+ dstd[i] = fi;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::undistortPoints
+
+void cv::Fisheye::initUndistortRectifyMap( InputArray _K, InputArray _D, InputArray _R, InputArray _P,
+ const cv::Size& size, int m1type, OutputArray map1, OutputArray map2 )
+{
+ CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 );
+ map1.create( size, m1type <= 0 ? CV_16SC2 : m1type );
+ map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F );
+
+ CV_Assert((_K.depth() == CV_32F || _K.depth() == CV_64F) && (_D.depth() == CV_32F || _D.depth() == CV_64F));
+ CV_Assert((_P.depth() == CV_32F || _P.depth() == CV_64F) && (_R.depth() == CV_32F || _R.depth() == CV_64F));
+ CV_Assert(_K.size() == Size(3, 3) && (_D.empty() || _D.total() == 4));
+ CV_Assert(_R.empty() || _R.size() == Size(3, 3) || _R.total() * _R.channels() == 3);
+ CV_Assert(_P.empty() || _P.size() == Size(3, 3) || _P.size() == Size(4, 3));
+
+ cv::Vec2d f, c;
+ if (_K.depth() == CV_32F)
+ {
+ Matx33f camMat = _K.getMat();
+ f = Vec2f(camMat(0, 0), camMat(1, 1));
+ c = Vec2f(camMat(0, 2), camMat(1, 2));
+ }
+ else
+ {
+ Matx33d camMat = _K.getMat();
+ f = Vec2d(camMat(0, 0), camMat(1, 1));
+ c = Vec2d(camMat(0, 2), camMat(1, 2));
+ }
+
+ Vec4d k = Vec4d::all(0);
+ if (!_D.empty())
+ k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr(): *_D.getMat().ptr();
+
+ cv::Matx33d R = cv::Matx33d::eye();
+ if (!_R.empty() && _R.total() * _R.channels() == 3)
+ {
+ cv::Vec3d rvec;
+ _R.getMat().convertTo(rvec, CV_64F);
+ R = Affine3d(rvec).rotation();
+ }
+ else if (!_R.empty() && _R.size() == Size(3, 3))
+ _R.getMat().convertTo(R, CV_64F);
+
+ cv::Matx33d P = cv::Matx33d::eye();
+ if (!_P.empty())
+ _P.getMat().colRange(0, 3).convertTo(P, CV_64F);
+
+ cv::Matx33d iR = (P * R).inv(cv::DECOMP_SVD);
+
+ for( int i = 0; i < size.height; ++i)
+ {
+ float* m1f = map1.getMat().ptr(i);
+ float* m2f = map2.getMat().ptr(i);
+ short* m1 = (short*)m1f;
+ ushort* m2 = (ushort*)m2f;
+
+ double _x = i*iR(0, 1) + iR(0, 2),
+ _y = i*iR(1, 1) + iR(1, 2),
+ _w = i*iR(2, 1) + iR(2, 2);
+
+ for( int j = 0; j < size.width; ++j)
+ {
+ double x = _x/_w, y = _y/_w;
+
+ double r = sqrt(x*x + y*y);
+ double theta = atan(r);
+
+ double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4;
+ double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8);
+
+ double scale = (r == 0) ? 1.0 : theta_d / r;
+ double u = f[0]*x*scale + c[0];
+ double v = f[1]*y*scale + c[1];
+
+ if( m1type == CV_16SC2 )
+ {
+ int iu = cv::saturate_cast(u*cv::INTER_TAB_SIZE);
+ int iv = cv::saturate_cast(v*cv::INTER_TAB_SIZE);
+ m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
+ m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
+ m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
+ }
+ else if( m1type == CV_32FC1 )
+ {
+ m1f[j] = (float)u;
+ m2f[j] = (float)v;
+ }
+
+ _x += iR(0, 0);
+ _y += iR(1, 0);
+ _w += iR(2, 0);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::undistortImage
+
+void cv::Fisheye::undistortImage(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray Knew, const Size& new_size)
+{
+ Size size = new_size.area() != 0 ? new_size : distorted.size();
+
+ cv::Mat map1, map2;
+ initUndistortRectifyMap(K, D, cv::Matx33d::eye(), Knew, size, CV_16SC2, map1, map2 );
+ cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::estimateNewCameraMatrixForUndistortRectify
+
+void cv::Fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
+ OutputArray P, double balance, const Size& new_size, double fov_scale)
+{
+ CV_Assert( K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F));
+ CV_Assert((D.empty() || D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F || D.empty()));
+
+ int w = image_size.width, h = image_size.height;
+ balance = std::min(std::max(balance, 0.0), 1.0);
+
+ cv::Mat points(1, 4, CV_64FC2);
+ Vec2d* pptr = points.ptr();
+ pptr[0] = Vec2d(w/2, 0);
+ pptr[1] = Vec2d(w, h/2);
+ pptr[2] = Vec2d(w/2, h);
+ pptr[3] = Vec2d(0, h/2);
+
+#if 0
+ const int N = 10;
+ cv::Mat points(1, N * 4, CV_64FC2);
+ Vec2d* pptr = points.ptr();
+ for(int i = 0, k = 0; i < 10; ++i)
+ {
+ pptr[k++] = Vec2d(w/2, 0) - Vec2d(w/8, 0) + Vec2d(w/4/N*i, 0);
+ pptr[k++] = Vec2d(w/2, h-1) - Vec2d(w/8, h-1) + Vec2d(w/4/N*i, h-1);
+
+ pptr[k++] = Vec2d(0, h/2) - Vec2d(0, h/8) + Vec2d(0, h/4/N*i);
+ pptr[k++] = Vec2d(w-1, h/2) - Vec2d(w-1, h/8) + Vec2d(w-1, h/4/N*i);
+ }
+#endif
+
+ undistortPoints(points, points, K, D, R);
+ cv::Scalar center_mass = mean(points);
+ cv::Vec2d cn(center_mass.val);
+
+ double aspect_ratio = (K.depth() == CV_32F) ? K.getMat().at(0,0)/K.getMat().at (1,1)
+ : K.getMat().at(0,0)/K.getMat().at(1,1);
+
+ // convert to identity ratio
+ cn[0] *= aspect_ratio;
+ for(size_t i = 0; i < points.total(); ++i)
+ pptr[i][1] *= aspect_ratio;
+
+ double minx = DBL_MAX, miny = DBL_MAX, maxx = -DBL_MAX, maxy = -DBL_MAX;
+ for(size_t i = 0; i < points.total(); ++i)
+ {
+ miny = std::min(miny, pptr[i][1]);
+ maxy = std::max(maxy, pptr[i][1]);
+ minx = std::min(minx, pptr[i][0]);
+ maxx = std::max(maxx, pptr[i][0]);
+ }
+
+#if 0
+ double minx = -DBL_MAX, miny = -DBL_MAX, maxx = DBL_MAX, maxy = DBL_MAX;
+ for(size_t i = 0; i < points.total(); ++i)
+ {
+ if (i % 4 == 0) miny = std::max(miny, pptr[i][1]);
+ if (i % 4 == 1) maxy = std::min(maxy, pptr[i][1]);
+ if (i % 4 == 2) minx = std::max(minx, pptr[i][0]);
+ if (i % 4 == 3) maxx = std::min(maxx, pptr[i][0]);
+ }
+#endif
+
+ double f1 = w * 0.5/(cn[0] - minx);
+ double f2 = w * 0.5/(maxx - cn[0]);
+ double f3 = h * 0.5 * aspect_ratio/(cn[1] - miny);
+ double f4 = h * 0.5 * aspect_ratio/(maxy - cn[1]);
+
+ double fmin = std::min(f1, std::min(f2, std::min(f3, f4)));
+ double fmax = std::max(f1, std::max(f2, std::max(f3, f4)));
+
+ double f = balance * fmin + (1.0 - balance) * fmax;
+ f *= fov_scale > 0 ? 1.0/fov_scale : 1.0;
+
+ cv::Vec2d new_f(f, f), new_c = -cn * f + Vec2d(w, h * aspect_ratio) * 0.5;
+
+ // restore aspect ratio
+ new_f[1] /= aspect_ratio;
+ new_c[1] /= aspect_ratio;
+
+ if (new_size.area() > 0)
+ {
+ double rx = new_size.width /(double)image_size.width;
+ double ry = new_size.height/(double)image_size.height;
+
+ new_f[0] *= rx; new_f[1] *= ry;
+ new_c[0] *= rx; new_c[1] *= ry;
+ }
+
+ Mat(Matx33d(new_f[0], 0, new_c[0],
+ 0, new_f[1], new_c[1],
+ 0, 0, 1)).convertTo(P, P.empty() ? K.type() : P.type());
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::stereoRectify
+
+void cv::Fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size& imageSize,
+ InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2,
+ OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale)
+{
+ CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F));
+ CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F));
+
+
+ cv::Mat aaa = _tvec.getMat().reshape(3, 1);
+
+ Vec3d rvec; // Rodrigues vector
+ if (_R.size() == Size(3, 3))
+ {
+ cv::Matx33d rmat;
+ _R.getMat().convertTo(rmat, CV_64F);
+ rvec = Affine3d(rmat).rvec();
+ }
+ else if (_R.total() * _R.channels() == 3)
+ _R.getMat().convertTo(rvec, CV_64F);
+
+ Vec3d tvec;
+ _tvec.getMat().convertTo(tvec, CV_64F);
+
+ // rectification algorithm
+ rvec *= -0.5; // get average rotation
+
+ Matx33d r_r;
+ Rodrigues(rvec, r_r); // rotate cameras to same orientation by averaging
+
+ Vec3d t = r_r * tvec;
+ Vec3d uu(t[0] > 0 ? 1 : -1, 0, 0);
+
+ // calculate global Z rotation
+ Vec3d ww = t.cross(uu);
+ double nw = norm(ww);
+ if (nw > 0.0)
+ ww *= acos(fabs(t[0])/cv::norm(t))/nw;
+
+ Matx33d wr;
+ Rodrigues(ww, wr);
+
+ // apply to both views
+ Matx33d ri1 = wr * r_r.t();
+ Mat(ri1, false).convertTo(R1, R1.empty() ? CV_64F : R1.type());
+ Matx33d ri2 = wr * r_r;
+ Mat(ri2, false).convertTo(R2, R2.empty() ? CV_64F : R2.type());
+ Vec3d tnew = ri2 * tvec;
+
+ // calculate projection/camera matrices. these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
+ Matx33d newK1, newK2;
+ estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale);
+ estimateNewCameraMatrixForUndistortRectify(K2, D2, imageSize, R2, newK2, balance, newImageSize, fov_scale);
+
+ double fc_new = std::min(newK1(1,1), newK2(1,1));
+ Point2d cc_new[2] = { Vec2d(newK1(0, 2), newK1(1, 2)), Vec2d(newK2(0, 2), newK2(1, 2)) };
+
+ // Vertical focal length must be the same for both images to keep the epipolar constraint use fy for fx also.
+ // For simplicity, set the principal points for both cameras to be the average
+ // of the two principal points (either one of or both x- and y- coordinates)
+ if( flags & cv::CALIB_ZERO_DISPARITY )
+ cc_new[0] = cc_new[1] = (cc_new[0] + cc_new[1]) * 0.5;
+ else
+ cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
+
+ Mat(Matx34d(fc_new, 0, cc_new[0].x, 0,
+ 0, fc_new, cc_new[0].y, 0,
+ 0, 0, 1, 0), false).convertTo(P1, P1.empty() ? CV_64F : P1.type());
+
+ Mat(Matx34d(fc_new, 0, cc_new[1].x, tnew[0]*fc_new, // baseline * focal length;,
+ 0, fc_new, cc_new[1].y, 0,
+ 0, 0, 1, 0), false).convertTo(P2, P2.empty() ? CV_64F : P2.type());
+
+ if (Q.needed())
+ Mat(Matx44d(1, 0, 0, -cc_new[0].x,
+ 0, 1, 0, -cc_new[0].y,
+ 0, 0, 0, fc_new,
+ 0, 0, -1./tnew[0], (cc_new[0].x - cc_new[1].x)/tnew[0]), false).convertTo(Q, Q.empty() ? CV_64F : Q.depth());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::calibrate
+
+double cv::Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
+ InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
+ int flags , cv::TermCriteria criteria)
+{
+ CV_Assert(!objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total());
+ CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
+ CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2);
+ CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty());
+ CV_Assert((!D.empty() && D.total() == 4) || D.empty());
+ CV_Assert((!rvecs.empty() && rvecs.channels() == 3) || rvecs.empty());
+ CV_Assert((!tvecs.empty() && tvecs.channels() == 3) || tvecs.empty());
+
+ CV_Assert(((flags & CALIB_USE_INTRINSIC_GUESS) && !K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS));
+
+ using namespace cv::internal;
+ //-------------------------------Initialization
+ IntrinsicParams finalParam;
+ IntrinsicParams currentParam;
+ IntrinsicParams errors;
+
+ finalParam.isEstimate[0] = 1;
+ finalParam.isEstimate[1] = 1;
+ finalParam.isEstimate[2] = 1;
+ finalParam.isEstimate[3] = 1;
+ finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1;
+ finalParam.isEstimate[5] = flags & CALIB_FIX_K1 ? 0 : 1;
+ finalParam.isEstimate[6] = flags & CALIB_FIX_K2 ? 0 : 1;
+ finalParam.isEstimate[7] = flags & CALIB_FIX_K3 ? 0 : 1;
+ finalParam.isEstimate[8] = flags & CALIB_FIX_K4 ? 0 : 1;
+
+ const int recompute_extrinsic = flags & CALIB_RECOMPUTE_EXTRINSIC ? 1: 0;
+ const int check_cond = flags & CALIB_CHECK_COND ? 1 : 0;
+
+ const double alpha_smooth = 0.4;
+ const double thresh_cond = 1e6;
+ double change = 1;
+ Vec2d err_std;
+
+ Matx33d _K;
+ Vec4d _D;
+ if (flags & CALIB_USE_INTRINSIC_GUESS)
+ {
+ K.getMat().convertTo(_K, CV_64FC1);
+ D.getMat().convertTo(_D, CV_64FC1);
+ finalParam.Init(Vec2d(_K(0,0), _K(1, 1)),
+ Vec2d(_K(0,2), _K(1, 2)),
+ Vec4d(flags & CALIB_FIX_K1 ? 0 : _D[0],
+ flags & CALIB_FIX_K2 ? 0 : _D[1],
+ flags & CALIB_FIX_K3 ? 0 : _D[2],
+ flags & CALIB_FIX_K4 ? 0 : _D[3]),
+ _K(0, 1) / _K(0, 0));
+ }
+ else
+ {
+ finalParam.Init(Vec2d(max(image_size.width, image_size.height) / CV_PI, max(image_size.width, image_size.height) / CV_PI),
+ Vec2d(image_size.width / 2.0 - 0.5, image_size.height / 2.0 - 0.5));
+ }
+
+ errors.isEstimate = finalParam.isEstimate;
+
+ std::vector omc(objectPoints.total()), Tc(objectPoints.total());
+
+ CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond, thresh_cond, omc, Tc);
+
+
+ //-------------------------------Optimization
+ for(int iter = 0; ; ++iter)
+ {
+ if ((criteria.type == 1 && iter >= criteria.maxCount) ||
+ (criteria.type == 2 && change <= criteria.epsilon) ||
+ (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
+ break;
+
+ double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0);
+
+ Mat JJ2_inv, ex3;
+ ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv, ex3);
+
+ Mat G = alpha_smooth2 * JJ2_inv * ex3;
+
+ currentParam = finalParam + G;
+
+ change = norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) -
+ Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1]))
+ / norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]));
+
+ finalParam = currentParam;
+
+ if (recompute_extrinsic)
+ {
+ CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond,
+ thresh_cond, omc, Tc);
+ }
+ }
+
+ //-------------------------------Validation
+ double rms;
+ EstimateUncertainties(objectPoints, imagePoints, finalParam, omc, Tc, errors, err_std, thresh_cond,
+ check_cond, rms);
+
+ //-------------------------------
+ _K = Matx33d(finalParam.f[0], finalParam.f[0] * finalParam.alpha, finalParam.c[0],
+ 0, finalParam.f[1], finalParam.c[1],
+ 0, 0, 1);
+
+ if (K.needed()) cv::Mat(_K).convertTo(K, K.empty() ? CV_64FC1 : K.type());
+ if (D.needed()) cv::Mat(finalParam.k).convertTo(D, D.empty() ? CV_64FC1 : D.type());
+ if (rvecs.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
+ if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
+
+ return rms;
+}
+
+namespace cv{ namespace {
+void subMatrix(const Mat& src, Mat& dst, const vector& cols, const vector& rows)
+{
+ CV_Assert(src.type() == CV_64FC1);
+
+ int nonzeros_cols = cv::countNonZero(cols);
+ Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
+
+ for (size_t i = 0, j = 0; i < cols.size(); i++)
+ {
+ if (cols[i])
+ {
+ src.col(i).copyTo(tmp.col(j++));
+ }
+ }
+
+ int nonzeros_rows = cv::countNonZero(rows);
+ Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1);
+ for (size_t i = 0, j = 0; i < rows.size(); i++)
+ {
+ if (rows[i])
+ {
+ tmp.row(i).copyTo(tmp1.row(j++));
+ }
+ }
+
+ dst = tmp1.clone();
+}
+
+}}
+
+cv::internal::IntrinsicParams::IntrinsicParams():
+ f(Vec2d::all(0)), c(Vec2d::all(0)), k(Vec4d::all(0)), alpha(0), isEstimate(9,0)
+{
+}
+
+cv::internal::IntrinsicParams::IntrinsicParams(Vec2d _f, Vec2d _c, Vec4d _k, double _alpha):
+ f(_f), c(_c), k(_k), alpha(_alpha), isEstimate(9,0)
+{
+}
+
+cv::internal::IntrinsicParams cv::internal::IntrinsicParams::operator+(const Mat& a)
+{
+ CV_Assert(a.type() == CV_64FC1);
+ IntrinsicParams tmp;
+ const double* ptr = a.ptr();
+
+ int j = 0;
+ tmp.f[0] = this->f[0] + (isEstimate[0] ? ptr[j++] : 0);
+ tmp.f[1] = this->f[1] + (isEstimate[1] ? ptr[j++] : 0);
+ tmp.c[0] = this->c[0] + (isEstimate[2] ? ptr[j++] : 0);
+ tmp.alpha = this->alpha + (isEstimate[4] ? ptr[j++] : 0);
+ tmp.c[1] = this->c[1] + (isEstimate[3] ? ptr[j++] : 0);
+ tmp.k[0] = this->k[0] + (isEstimate[5] ? ptr[j++] : 0);
+ tmp.k[1] = this->k[1] + (isEstimate[6] ? ptr[j++] : 0);
+ tmp.k[2] = this->k[2] + (isEstimate[7] ? ptr[j++] : 0);
+ tmp.k[3] = this->k[3] + (isEstimate[8] ? ptr[j++] : 0);
+
+ tmp.isEstimate = isEstimate;
+ return tmp;
+}
+
+cv::internal::IntrinsicParams& cv::internal::IntrinsicParams::operator =(const Mat& a)
+{
+ CV_Assert(a.type() == CV_64FC1);
+ const double* ptr = a.ptr();
+
+ int j = 0;
+
+ this->f[0] = isEstimate[0] ? ptr[j++] : 0;
+ this->f[1] = isEstimate[1] ? ptr[j++] : 0;
+ this->c[0] = isEstimate[2] ? ptr[j++] : 0;
+ this->c[1] = isEstimate[3] ? ptr[j++] : 0;
+ this->alpha = isEstimate[4] ? ptr[j++] : 0;
+ this->k[0] = isEstimate[5] ? ptr[j++] : 0;
+ this->k[1] = isEstimate[6] ? ptr[j++] : 0;
+ this->k[2] = isEstimate[7] ? ptr[j++] : 0;
+ this->k[3] = isEstimate[8] ? ptr[j++] : 0;
+
+ return *this;
+}
+
+void cv::internal::IntrinsicParams::Init(const cv::Vec2d& _f, const cv::Vec2d& _c, const cv::Vec4d& _k, const double& _alpha)
+{
+ this->c = _c;
+ this->f = _f;
+ this->k = _k;
+ this->alpha = _alpha;
+}
+
+void cv::internal::projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
+ cv::InputArray _rvec,cv::InputArray _tvec,
+ const IntrinsicParams& param, cv::OutputArray jacobian)
+{
+ CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
+ Matx33d K(param.f[0], param.f[0] * param.alpha, param.c[0],
+ 0, param.f[1], param.c[1],
+ 0, 0, 1);
+ Fisheye::projectPoints(objectPoints, imagePoints, _rvec, _tvec, K, param.k, param.alpha, jacobian);
+}
+
+void cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
+ Mat& tvec, Mat& J, const int MaxIter,
+ const IntrinsicParams& param, const double thresh_cond)
+{
+ CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
+ CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
+ Vec6d extrinsics(rvec.at(0), rvec.at(1), rvec.at(2),
+ tvec.at(0), tvec.at(1), tvec.at(2));
+ double change = 1;
+ int iter = 0;
+
+ while (change > 1e-10 && iter < MaxIter)
+ {
+ std::vector x;
+ Mat jacobians;
+ projectPoints(objectPoints, x, rvec, tvec, param, jacobians);
+
+ Mat ex = imagePoints - Mat(x).t();
+ ex = ex.reshape(1, 2);
+
+ J = jacobians.colRange(8, 14).clone();
+
+ SVD svd(J, SVD::NO_UV);
+ double condJJ = svd.w.at(0)/svd.w.at(5);
+
+ if (condJJ > thresh_cond)
+ change = 0;
+ else
+ {
+ Vec6d param_innov;
+ solve(J, ex.reshape(1, (int)ex.total()), param_innov, DECOMP_SVD + DECOMP_NORMAL);
+
+ Vec6d param_up = extrinsics + param_innov;
+ change = norm(param_innov)/norm(param_up);
+ extrinsics = param_up;
+ iter = iter + 1;
+
+ rvec = Mat(Vec3d(extrinsics.val));
+ tvec = Mat(Vec3d(extrinsics.val+3));
+ }
+ }
+}
+
+cv::Mat cv::internal::ComputeHomography(Mat m, Mat M)
+{
+ int Np = m.cols;
+
+ if (m.rows < 3)
+ {
+ vconcat(m, Mat::ones(1, Np, CV_64FC1), m);
+ }
+ if (M.rows < 3)
+ {
+ vconcat(M, Mat::ones(1, Np, CV_64FC1), M);
+ }
+
+ divide(m, Mat::ones(3, 1, CV_64FC1) * m.row(2), m);
+ divide(M, Mat::ones(3, 1, CV_64FC1) * M.row(2), M);
+
+ Mat ax = m.row(0).clone();
+ Mat ay = m.row(1).clone();
+
+ double mxx = mean(ax)[0];
+ double myy = mean(ay)[0];
+
+ ax = ax - mxx;
+ ay = ay - myy;
+
+ double scxx = mean(abs(ax))[0];
+ double scyy = mean(abs(ay))[0];
+
+ Mat Hnorm (Matx33d( 1/scxx, 0.0, -mxx/scxx,
+ 0.0, 1/scyy, -myy/scyy,
+ 0.0, 0.0, 1.0 ));
+
+ Mat inv_Hnorm (Matx33d( scxx, 0, mxx,
+ 0, scyy, myy,
+ 0, 0, 1 ));
+ Mat mn = Hnorm * m;
+
+ Mat L = Mat::zeros(2*Np, 9, CV_64FC1);
+
+ for (int i = 0; i < Np; ++i)
+ {
+ for (int j = 0; j < 3; j++)
+ {
+ L.at(2 * i, j) = M.at(j, i);
+ L.at(2 * i + 1, j + 3) = M.at(j, i);
+ L.at(2 * i, j + 6) = -mn.at(0,i) * M.at(j, i);
+ L.at(2 * i + 1, j + 6) = -mn.at(1,i) * M.at(j, i);
+ }
+ }
+
+ if (Np > 4) L = L.t() * L;
+ SVD svd(L);
+ Mat hh = svd.vt.row(8) / svd.vt.row(8).at(8);
+ Mat Hrem = hh.reshape(1, 3);
+ Mat H = inv_Hnorm * Hrem;
+
+ if (Np > 4)
+ {
+ Mat hhv = H.reshape(1, 9)(Rect(0, 0, 1, 8)).clone();
+ for (int iter = 0; iter < 10; iter++)
+ {
+ Mat mrep = H * M;
+ Mat J = Mat::zeros(2 * Np, 8, CV_64FC1);
+ Mat MMM;
+ divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), MMM);
+ divide(mrep, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), mrep);
+ Mat m_err = m(Rect(0,0, m.cols, 2)) - mrep(Rect(0,0, mrep.cols, 2));
+ m_err = Mat(m_err.t()).reshape(1, m_err.cols * m_err.rows);
+ Mat MMM2, MMM3;
+ multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 0, mrep.cols, 1)), MMM, MMM2);
+ multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 1, mrep.cols, 1)), MMM, MMM3);
+
+ for (int i = 0; i < Np; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ J.at(2 * i, j) = -MMM.at(j, i);
+ J.at(2 * i + 1, j + 3) = -MMM.at(j, i);
+ }
+
+ for (int j = 0; j < 2; ++j)
+ {
+ J.at(2 * i, j + 6) = MMM2.at(j, i);
+ J.at(2 * i + 1, j + 6) = MMM3.at(j, i);
+ }
+ }
+ divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0,2,mrep.cols,1)), MMM);
+ Mat hh_innov = (J.t() * J).inv() * (J.t()) * m_err;
+ Mat hhv_up = hhv - hh_innov;
+ Mat tmp;
+ vconcat(hhv_up, Mat::ones(1,1,CV_64FC1), tmp);
+ Mat H_up = tmp.reshape(1,3);
+ hhv = hhv_up;
+ H = H_up;
+ }
+ }
+ return H;
+}
+
+cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param)
+{
+ CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
+
+ Mat distorted((int)imagePoints.total(), 1, CV_64FC2), undistorted;
+ const Vec2d* ptr = imagePoints.ptr(0);
+ Vec2d* ptr_d = distorted.ptr(0);
+ for (size_t i = 0; i < imagePoints.total(); ++i)
+ {
+ ptr_d[i] = (ptr[i] - param.c).mul(Vec2d(1.0 / param.f[0], 1.0 / param.f[1]));
+ ptr_d[i][0] = ptr_d[i][0] - param.alpha * ptr_d[i][1];
+ }
+ cv::Fisheye::undistortPoints(distorted, undistorted, Matx33d::eye(), param.k);
+ return undistorted;
+}
+
+void cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk)
+{
+
+ CV_Assert(!_objectPoints.empty() && _objectPoints.type() == CV_64FC3);
+ CV_Assert(!_imagePoints.empty() && _imagePoints.type() == CV_64FC2);
+
+ Mat imagePointsNormalized = NormalizePixels(_imagePoints.t(), param).reshape(1).t();
+ Mat objectPoints = Mat(_objectPoints.t()).reshape(1).t();
+ Mat objectPointsMean, covObjectPoints;
+ Mat Rckk;
+ int Np = imagePointsNormalized.cols;
+ calcCovarMatrix(objectPoints, covObjectPoints, objectPointsMean, CV_COVAR_NORMAL | CV_COVAR_COLS);
+ SVD svd(covObjectPoints);
+ Mat R(svd.vt);
+ if (norm(R(Rect(2, 0, 1, 2))) < 1e-6)
+ R = Mat::eye(3,3, CV_64FC1);
+ if (determinant(R) < 0)
+ R = -R;
+ Mat T = -R * objectPointsMean;
+ Mat X_new = R * objectPoints + T * Mat::ones(1, Np, CV_64FC1);
+ Mat H = ComputeHomography(imagePointsNormalized, X_new(Rect(0,0,X_new.cols,2)));
+ double sc = .5 * (norm(H.col(0)) + norm(H.col(1)));
+ H = H / sc;
+ Mat u1 = H.col(0).clone();
+ u1 = u1 / norm(u1);
+ Mat u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1;
+ u2 = u2 / norm(u2);
+ Mat u3 = u1.cross(u2);
+ Mat RRR;
+ hconcat(u1, u2, RRR);
+ hconcat(RRR, u3, RRR);
+ Rodrigues(RRR, omckk);
+ Rodrigues(omckk, Rckk);
+ Tckk = H.col(2).clone();
+ Tckk = Tckk + Rckk * T;
+ Rckk = Rckk * R;
+ Rodrigues(Rckk, omckk);
+}
+
+void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, const int check_cond,
+ const double thresh_cond, InputOutputArray omc, InputOutputArray Tc)
+{
+ CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
+ CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
+ CV_Assert(omc.type() == CV_64FC3 || Tc.type() == CV_64FC3);
+
+ if (omc.empty()) omc.create(1, (int)objectPoints.total(), CV_64FC3);
+ if (Tc.empty()) Tc.create(1, (int)objectPoints.total(), CV_64FC3);
+
+ const int maxIter = 20;
+
+ for(int image_idx = 0; image_idx < (int)imagePoints.total(); ++image_idx)
+ {
+ Mat omckk, Tckk, JJ_kk;
+ Mat image, object;
+
+ objectPoints.getMat(image_idx).convertTo(object, CV_64FC3);
+ imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);
+
+ InitExtrinsics(image, object, param, omckk, Tckk);
+
+ ComputeExtrinsicRefine(image, object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond);
+ if (check_cond)
+ {
+ SVD svd(JJ_kk, SVD::NO_UV);
+ if (svd.w.at(0) / svd.w.at((int)svd.w.total() - 1) > thresh_cond)
+ {
+ CV_Assert(!"cond > thresh_cond");
+ }
+ }
+ omckk.reshape(3,1).copyTo(omc.getMat().col(image_idx));
+ Tckk.reshape(3,1).copyTo(Tc.getMat().col(image_idx));
+ }
+}
+
+
+void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, InputArray omc, InputArray Tc,
+ const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3)
+{
+ CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
+ CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
+
+ CV_Assert(!omc.empty() && omc.type() == CV_64FC3);
+ CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3);
+
+ int n = (int)objectPoints.total();
+
+ Mat JJ3 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1);
+ ex3 = Mat::zeros(9 + 6 * n, 1, CV_64FC1 );
+
+ for (int image_idx = 0; image_idx < n; ++image_idx)
+ {
+ Mat image, object;
+ objectPoints.getMat(image_idx).convertTo(object, CV_64FC3);
+ imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);
+
+ Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx));
+
+ std::vector x;
+ Mat jacobians;
+ projectPoints(object, x, om, T, param, jacobians);
+ Mat exkk = image.t() - Mat(x);
+
+ Mat A(jacobians.rows, 9, CV_64FC1);
+ jacobians.colRange(0, 4).copyTo(A.colRange(0, 4));
+ jacobians.col(14).copyTo(A.col(4));
+ jacobians.colRange(4, 8).copyTo(A.colRange(5, 9));
+
+ A = A.t();
+
+ Mat B = jacobians.colRange(8, 14).clone();
+ B = B.t();
+
+ JJ3(Rect(0, 0, 9, 9)) = JJ3(Rect(0, 0, 9, 9)) + A * A.t();
+ JJ3(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();
+
+ Mat AB = A * B.t();
+ AB.copyTo(JJ3(Rect(9 + 6 * image_idx, 0, 6, 9)));
+
+ JJ3(Rect(0, 9 + 6 * image_idx, 9, 6)) = AB.t();
+ ex3(Rect(0,0,1,9)) = ex3(Rect(0,0,1,9)) + A * exkk.reshape(1, 2 * exkk.rows);
+
+ ex3(Rect(0, 9 + 6 * image_idx, 1, 6)) = B * exkk.reshape(1, 2 * exkk.rows);
+
+ if (check_cond)
+ {
+ Mat JJ_kk = B.t();
+ SVD svd(JJ_kk, SVD::NO_UV);
+ double cond = svd.w.at(0) / svd.w.at(svd.w.rows - 1);
+ if (cond > thresh_cond)
+ {
+ CV_Assert(!"cond > thresh_cond");
+ }
+ }
+ }
+
+ vector idxs(param.isEstimate);
+ idxs.insert(idxs.end(), 6 * n, 1);
+
+ subMatrix(JJ3, JJ3, idxs, idxs);
+ subMatrix(ex3, ex3, std::vector(1, 1), idxs);
+ JJ2_inv = JJ3.inv();
+}
+
+void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& params, InputArray omc, InputArray Tc,
+ IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms)
+{
+ CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
+ CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
+
+ CV_Assert(!omc.empty() && omc.type() == CV_64FC3);
+ CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3);
+
+ Mat ex((int)(objectPoints.getMat(0).total() * objectPoints.total()), 1, CV_64FC2);
+
+ for (size_t image_idx = 0; image_idx < objectPoints.total(); ++image_idx)
+ {
+ Mat image, object;
+ objectPoints.getMat(image_idx).convertTo(object, CV_64FC3);
+ imagePoints.getMat (image_idx).convertTo(image, CV_64FC2);
+
+ Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx));
+
+ std::vector x;
+ projectPoints(object, x, om, T, params, noArray());
+ Mat ex_ = image.t() - Mat(x);
+ ex_.copyTo(ex.rowRange(ex_.rows * image_idx, ex_.rows * (image_idx + 1)));
+ }
+
+ meanStdDev(ex, noArray(), std_err);
+ std_err *= sqrt(ex.total()/(ex.total() - 1.0));
+
+ Mat sigma_x;
+ meanStdDev(ex.reshape(1, 1), noArray(), sigma_x);
+ sigma_x *= sqrt(2 * ex.total()/(2 * ex.total() - 1.0));
+
+ Mat _JJ2_inv, ex3;
+ ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, _JJ2_inv, ex3);
+
+ Mat_& JJ2_inv = (Mat_&)_JJ2_inv;
+
+ sqrt(JJ2_inv, JJ2_inv);
+
+ double s = sigma_x.at(0);
+ Mat r = 3 * s * JJ2_inv.diag();
+ errors = r;
+
+ rms = 0;
+ const Vec2d* ptr_ex = ex.ptr();
+ for (size_t i = 0; i < ex.total(); i++)
+ {
+ rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
+ }
+
+ rms /= ex.total();
+ rms = sqrt(rms);
+}
diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp
new file mode 100644
index 0000000000..3989e1bc2f
--- /dev/null
+++ b/modules/calib3d/test/test_fisheye.cpp
@@ -0,0 +1,423 @@
+#include "test_precomp.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#define DEF_PARAM_TEST(name, ...) typedef ::perf::TestBaseWithParam< std::tr1::tuple< __VA_ARGS__ > > name
+#define PARAM_TEST_CASE(name, ...) struct name : testing::TestWithParam< std::tr1::tuple< __VA_ARGS__ > >
+
+
+namespace FishEye
+{
+ const static cv::Size imageSize(1280, 800);
+
+ const static cv::Matx33d K(558.478087865323, 0, 620.458515360843,
+ 0, 560.506767351568, 381.939424848348,
+ 0, 0, 1);
+
+ const static cv::Vec4d D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
+
+ const static cv::Matx33d R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
+ -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
+ -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
+
+ const static cv::Vec3d T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
+}
+
+namespace{
+std::string combine(const std::string& _item1, const std::string& _item2)
+{
+ std::string item1 = _item1, item2 = _item2;
+ std::replace(item1.begin(), item1.end(), '\\', '/');
+ std::replace(item2.begin(), item2.end(), '\\', '/');
+
+ if (item1.empty())
+ return item2;
+
+ if (item2.empty())
+ return item1;
+
+ char last = item1[item1.size()-1];
+ return item1 + (last != '/' ? "/" : "") + item2;
+}
+
+std::string combine_format(const std::string& item1, const std::string& item2, ...)
+{
+ std::string fmt = combine(item1, item2);
+ char buffer[1 << 16];
+ va_list args;
+ va_start( args, item2 );
+ vsprintf( buffer, fmt.c_str(), args );
+ va_end( args );
+ return std::string(buffer);
+}
+
+void readPoins(std::vector >& objectPoints,
+ std::vector >& imagePoints,
+ const std::string& path, const int n_images, const int n_points)
+{
+ objectPoints.resize(n_images);
+ imagePoints.resize(n_images);
+
+ std::vector image(n_points);
+ std::vector object(n_points);
+
+ std::ifstream ipStream;
+ std::ifstream opStream;
+
+ for (int image_idx = 0; image_idx < n_images; image_idx++)
+ {
+ std::stringstream ss;
+ ss << image_idx;
+ std::string idxStr = ss.str();
+
+ ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
+ opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
+ CV_Assert(ipStream.is_open() && opStream.is_open());
+
+ for (int point_idx = 0; point_idx < n_points; point_idx++)
+ {
+ double x, y, z;
+ char delim;
+ ipStream >> x >> delim >> y;
+ image[point_idx] = cv::Point2d(x, y);
+ opStream >> x >> delim >> y >> delim >> z;
+ object[point_idx] = cv::Point3d(x, y, z);
+ }
+ ipStream.close();
+ opStream.close();
+
+ imagePoints[image_idx] = image;
+ objectPoints[image_idx] = object;
+ }
+}
+
+void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
+ cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q)
+{
+ cv::FileStorage fs(file, cv::FileStorage::READ);
+ CV_Assert(fs.isOpened());
+
+ cv::Mat R, T, R1, R2, P1, P2, Q;
+ fs["R"] >> R; fs["T"] >> T; fs["R1"] >> R1; fs["R2"] >> R2; fs["P1"] >> P1; fs["P2"] >> P2; fs["Q"] >> Q;
+ if (_R.needed()) R.copyTo(_R); if(_T.needed()) T.copyTo(_T); if (_R1.needed()) R1.copyTo(_R1); if (_R2.needed()) R2.copyTo(_R2);
+ if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q);
+}
+
+cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r, double scale)
+{
+ CV_Assert(l.type() == r.type() && l.size() == r.size());
+ cv::Mat merged(l.rows, l.cols * 2, l.type());
+ cv::Mat lpart = merged.colRange(0, l.cols);
+ cv::Mat rpart = merged.colRange(l.cols, merged.cols);
+ l.copyTo(lpart);
+ r.copyTo(rpart);
+
+ for(int i = 0; i < l.rows; i+=20)
+ cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0));
+
+ return merged;
+}
+
+}
+
+
+
+/// Change this parameter via CMake: cmake -DDATASETS_REPOSITORY_FOLDER=
+//const static std::string datasets_repository_path = "DATASETS_REPOSITORY_FOLDER";
+const static std::string datasets_repository_path = "/home/krylov/data";
+
+TEST(FisheyeTest, projectPoints)
+{
+ double cols = FishEye::imageSize.width,
+ rows = FishEye::imageSize.height;
+
+ const int N = 20;
+ cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2;
+ undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3));
+ cv::Vec2d* pts = distorted0.ptr();
+
+ cv::Vec2d c(FishEye::K(0, 2), FishEye::K(1, 2));
+ for(int y = 0, k = 0; y < N; ++y)
+ for(int x = 0; x < N; ++x)
+ {
+ cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f));
+ pts[k++] = (point - c) * 0.85 + c;
+ }
+
+ cv::Fisheye::undistortPoints(distorted0, undist1, FishEye::K, FishEye::D);
+
+ cv::Vec2d* u1 = undist1.ptr();
+ cv::Vec3d* u2 = undist2.ptr();
+ for(int i = 0; i < (int)distorted0.total(); ++i)
+ u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0);
+
+ cv::Fisheye::distortPoints(undist1, distorted1, FishEye::K, FishEye::D);
+ cv::Fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), FishEye::K, FishEye::D);
+
+ EXPECT_MAT_NEAR(distorted0, distorted1, 1e-5);
+ EXPECT_MAT_NEAR(distorted0, distorted2, 1e-5);
+}
+
+TEST(FisheyeTest, undistortImage)
+{
+ cv::Matx33d K = FishEye::K;
+ cv::Mat D = cv::Mat(FishEye::D);
+ std::string file = combine(datasets_repository_path, "image000001.png");
+
+ cv::Matx33d newK = K;
+ cv::Mat distorted = cv::imread(file), undistorted;
+
+ {
+ newK(0, 0) = 100;
+ newK(1, 1) = 100;
+ cv::Fisheye::undistortImage(distorted, undistorted, K, D, newK);
+ cv::Mat correct = cv::imread(combine(datasets_repository_path, "test_undistortImage/new_f_100.png"));
+ if (correct.empty())
+ CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/new_f_100.png"), undistorted));
+ else
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ }
+ {
+ double balance = 1.0;
+ cv::Fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
+ cv::Fisheye::undistortImage(distorted, undistorted, K, D, newK);
+ cv::Mat correct = cv::imread(combine(datasets_repository_path, "test_undistortImage/balance_1.0.png"));
+ if (correct.empty())
+ CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_1.0.png"), undistorted));
+ else
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ }
+
+ {
+ double balance = 0.0;
+ cv::Fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
+ cv::Fisheye::undistortImage(distorted, undistorted, K, D, newK);
+ cv::Mat correct = cv::imread(combine(datasets_repository_path, "test_undistortImage/balance_0.0.png"));
+ if (correct.empty())
+ CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_0.0.png"), undistorted));
+ else
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ }
+
+ cv::waitKey();
+}
+
+TEST(FisheyeTest, jacobians)
+{
+ int n = 10;
+ cv::Mat X(1, n, CV_32FC4);
+ cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F);
+ cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
+ cv::Mat k(4, 1, CV_64F);
+ double alpha;
+
+ cv::RNG& r = cv::theRNG();
+
+ r.fill(X, cv::RNG::NORMAL, 0, 1);
+ X = cv::abs(X) * 10;
+
+ r.fill(om, cv::RNG::NORMAL, 0, 1);
+ om = cv::abs(om);
+
+ r.fill(T, cv::RNG::NORMAL, 0, 1);
+ T = cv::abs(T); T.at(2) = 4; T *= 10;
+
+ r.fill(f, cv::RNG::NORMAL, 0, 1);
+ f = cv::abs(f) * 1000;
+
+ r.fill(c, cv::RNG::NORMAL, 0, 1);
+ c = cv::abs(c) * 1000;
+
+ r.fill(k, cv::RNG::NORMAL, 0, 1);
+ k*= 0.5;
+
+ alpha = 0.01*r.gaussian(1);
+
+
+ CV_Assert(!"/////////");
+}
+
+TEST(FisheyeTest, Calibration)
+{
+ const int n_images = 34;
+ const int n_points = 48;
+
+ cv::Size imageSize = cv::Size(1280, 800);
+ std::vector > imagePoints;
+ std::vector > objectPoints;
+
+ readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+
+ int flag = 0;
+ flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+ flag |= cv::Fisheye::CALIB_CHECK_COND;
+ flag |= cv::Fisheye::CALIB_FIX_SKEW;
+
+ cv::Matx33d K;
+ cv::Vec4d D;
+
+ cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
+ cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
+
+ EXPECT_MAT_NEAR(K, FishEye::K, 1e-11);
+ EXPECT_MAT_NEAR(D, FishEye::D, 1e-12);
+}
+
+TEST(FisheyeTest, Homography)
+{
+ const int n_images = 1;
+ const int n_points = 48;
+
+ cv::Size imageSize = cv::Size(1280, 800);
+ std::vector > imagePoints;
+ std::vector > objectPoints;
+
+ readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+ cv::internal::IntrinsicParams param;
+ param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
+ cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
+
+ cv::Mat _imagePoints (imagePoints[0]);
+ cv::Mat _objectPoints(objectPoints[0]);
+
+ cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
+ _objectPoints = _objectPoints.reshape(1).t();
+ cv::Mat objectPointsMean, covObjectPoints;
+
+ int Np = imagePointsNormalized.cols;
+ cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, CV_COVAR_NORMAL | CV_COVAR_COLS);
+ cv::SVD svd(covObjectPoints);
+ cv::Mat R(svd.vt);
+
+ if (cv::norm(R(cv::Rect(2, 0, 1, 2))) < 1e-6)
+ R = cv::Mat::eye(3,3, CV_64FC1);
+ if (cv::determinant(R) < 0)
+ R = -R;
+
+ cv::Mat T = -R * objectPointsMean;
+ cv::Mat X_new = R * _objectPoints + T * cv::Mat::ones(1, Np, CV_64FC1);
+ cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));
+
+ cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
+ X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2));
+ cv::Mat mrep = H * M;
+
+ cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep);
+
+ cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t();
+
+ cv::Vec2d std_err;
+ cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err);
+ std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
+
+ cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
+ EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-16);
+}
+
+TEST(TestFisheye, EtimateUncertainties)
+{
+ const int n_images = 34;
+ const int n_points = 48;
+
+ cv::Size imageSize = cv::Size(1280, 800);
+ std::vector > imagePoints;
+ std::vector > objectPoints;
+
+ readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+
+ int flag = 0;
+ flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+ flag |= cv::Fisheye::CALIB_CHECK_COND;
+ flag |= cv::Fisheye::CALIB_FIX_SKEW;
+
+ cv::Matx33d K;
+ cv::Vec4d D;
+ std::vector rvec;
+ std::vector tvec;
+
+ cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
+ cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
+
+ cv::internal::IntrinsicParams param, errors;
+ cv::Vec2d err_std;
+ double thresh_cond = 1e6;
+ int check_cond = 1;
+ param.Init(cv::Vec2d(K(0,0), K(1,1)), cv::Vec2d(K(0,2), K(1, 2)), D);
+ param.isEstimate = std::vector(9, 1);
+ param.isEstimate[4] = 0;
+
+ errors.isEstimate = param.isEstimate;
+
+ double rms;
+
+ cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec,
+ errors, err_std, thresh_cond, check_cond, rms);
+
+ EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-14);
+ EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-15);
+ EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-15);
+ EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-15);
+ CV_Assert(abs(rms - 0.263782587133546) < 1e-15);
+ CV_Assert(errors.alpha == 0);
+ }
+
+TEST(FisheyeTest, rectify)
+{
+ const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+
+ cv::Size calibration_size = FishEye::imageSize, requested_size = calibration_size;
+ cv::Matx33d K1 = FishEye::K, K2 = K1;
+ cv::Mat D1 = cv::Mat(FishEye::D), D2 = D1;
+
+ cv::Vec3d T = FishEye::T;
+ cv::Matx33d R = FishEye::R;
+
+ double balance = 0.0, fov_scale = 1.1;
+ cv::Mat R1, R2, P1, P2, Q;
+ cv::Fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, R, T, R1, R2, P1, P2, Q,
+ cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale);
+
+ cv::Mat lmapx, lmapy, rmapx, rmapy;
+ //rewrite for fisheye
+ cv::Fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
+ cv::Fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);
+
+ cv::Mat l, r, lundist, rundist;
+ cv::VideoCapture lcap(combine(folder, "left/stereo_pair_%03d.jpg")),
+ rcap(combine(folder, "right/stereo_pair_%03d.jpg"));
+
+ for(int i = 0;; ++i)
+ {
+ lcap >> l; rcap >> r;
+ if (l.empty() || r.empty())
+ break;
+
+ int ndisp = 128;
+ cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), CV_RGB(255, 0, 0));
+ cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), CV_RGB(255, 0, 0));
+ cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), CV_RGB(255, 0, 0));
+ cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
+ cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
+
+ cv::Mat rectification = mergeRectification(lundist, rundist, 0.75);
+
+ cv::Mat correct = cv::imread(combine_format(folder, "test_rectify/rectification_AB_%03d.png", i));
+ if (correct.empty())
+ cv::imwrite(combine_format(folder, "test_rectify/rectification_AB_%03d.png", i), rectification);
+ else
+ EXPECT_MAT_NEAR(correct, rectification, 1e-15);
+ }
+}
+
+
+
+
+
From 35e1b322cbc9640fc60ab03b6fc5783782c6ca1b Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Fri, 25 Apr 2014 18:36:48 +0400
Subject: [PATCH 004/189] Added test for jacobians
---
modules/calib3d/test/test_fisheye.cpp | 91 ++++++++++++++++++++-------
1 file changed, 70 insertions(+), 21 deletions(-)
diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp
index 3989e1bc2f..b2f47987cb 100644
--- a/modules/calib3d/test/test_fisheye.cpp
+++ b/modules/calib3d/test/test_fisheye.cpp
@@ -1,18 +1,13 @@
#include "test_precomp.hpp"
-
-#include
-#include
-#include
-#include
-#include
-
+#include
#include
-#include
-#include
#define DEF_PARAM_TEST(name, ...) typedef ::perf::TestBaseWithParam< std::tr1::tuple< __VA_ARGS__ > > name
#define PARAM_TEST_CASE(name, ...) struct name : testing::TestWithParam< std::tr1::tuple< __VA_ARGS__ > >
+/// Change this parameter via CMake: cmake -DDATASETS_REPOSITORY_FOLDER=
+//const static std::string datasets_repository_path = "DATASETS_REPOSITORY_FOLDER";
+const static std::string datasets_repository_path = "/home/krylov/data";
namespace FishEye
{
@@ -111,7 +106,7 @@ void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray
if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q);
}
-cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r, double scale)
+cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r)
{
CV_Assert(l.type() == r.type() && l.size() == r.size());
cv::Mat merged(l.rows, l.cols * 2, l.type());
@@ -128,12 +123,6 @@ cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r, double scale)
}
-
-
-/// Change this parameter via CMake: cmake -DDATASETS_REPOSITORY_FOLDER=
-//const static std::string datasets_repository_path = "DATASETS_REPOSITORY_FOLDER";
-const static std::string datasets_repository_path = "/home/krylov/data";
-
TEST(FisheyeTest, projectPoints)
{
double cols = FishEye::imageSize.width,
@@ -213,7 +202,7 @@ TEST(FisheyeTest, undistortImage)
TEST(FisheyeTest, jacobians)
{
int n = 10;
- cv::Mat X(1, n, CV_32FC4);
+ cv::Mat X(1, n, CV_64FC3);
cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F);
cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
cv::Mat k(4, 1, CV_64F);
@@ -221,7 +210,7 @@ TEST(FisheyeTest, jacobians)
cv::RNG& r = cv::theRNG();
- r.fill(X, cv::RNG::NORMAL, 0, 1);
+ r.fill(X, cv::RNG::NORMAL, 2, 1);
X = cv::abs(X) * 10;
r.fill(om, cv::RNG::NORMAL, 0, 1);
@@ -241,8 +230,68 @@ TEST(FisheyeTest, jacobians)
alpha = 0.01*r.gaussian(1);
-
- CV_Assert(!"/////////");
+ cv::Mat x1, x2, xpred;
+ cv::Matx33d K(f.at(0), alpha * f.at(0), c.at(0),
+ 0, f.at(1), c.at(1),
+ 0, 0, 1);
+
+ cv::Mat jacobians;
+ cv::Fisheye::projectPoints(X, x1, om, T, K, k, alpha, jacobians);
+
+ //test on T:
+ cv::Mat dT(3, 1, CV_64FC1);
+ r.fill(dT, cv::RNG::NORMAL, 0, 1);
+ dT *= 1e-9*cv::norm(T);
+ cv::Mat T2 = T + dT;
+ cv::Fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+
+ //test on om:
+ cv::Mat dom(3, 1, CV_64FC1);
+ r.fill(dom, cv::RNG::NORMAL, 0, 1);
+ dom *= 1e-9*cv::norm(om);
+ cv::Mat om2 = om + dom;
+ cv::Fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+
+ //test on f:
+ cv::Mat df(2, 1, CV_64FC1);
+ r.fill(df, cv::RNG::NORMAL, 0, 1);
+ df *= 1e-9*cv::norm(f);
+ cv::Matx33d K2 = K + cv::Matx33d(df.at(0), df.at(0) * alpha, 0, 0, df.at(1), 0, 0, 0, 0);
+ cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+
+ //test on c:
+ cv::Mat dc(2, 1, CV_64FC1);
+ r.fill(dc, cv::RNG::NORMAL, 0, 1);
+ dc *= 1e-9*cv::norm(c);
+ K2 = K + cv::Matx33d(0, 0, dc.at(0), 0, 0, dc.at(1), 0, 0, 0);
+ cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+
+ //test on k:
+ cv::Mat dk(4, 1, CV_64FC1);
+ r.fill(dk, cv::RNG::NORMAL, 0, 1);
+ dk *= 1e-9*cv::norm(k);
+ cv::Mat k2 = k + dk;
+ cv::Fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+
+ //test on alpha:
+ cv::Mat dalpha(1, 1, CV_64FC1);
+ r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
+ dalpha *= 1e-9*cv::norm(f);
+ double alpha2 = alpha + dalpha.at(0);
+ K2 = K + cv::Matx33d(0, f.at(0) * dalpha.at(0), 0, 0, 0, 0, 0, 0, 0);
+ cv::Fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray());
+ xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-12);
}
TEST(FisheyeTest, Calibration)
@@ -407,7 +456,7 @@ TEST(FisheyeTest, rectify)
cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
- cv::Mat rectification = mergeRectification(lundist, rundist, 0.75);
+ cv::Mat rectification = mergeRectification(lundist, rundist);
cv::Mat correct = cv::imread(combine_format(folder, "test_rectify/rectification_AB_%03d.png", i));
if (correct.empty())
From e6420bde73f57cdbf1024d0b39049f22df3d9414 Mon Sep 17 00:00:00 2001
From: Samson Yilma
Date: Sat, 26 Apr 2014 20:13:27 -0400
Subject: [PATCH 005/189] Added function decomposeHomographyMat. New files
added are homography_decomp.cpp and test_homography_decomp.cpp. Modified
files calib3d.hpp and camera_calibration_and_3d_reconstruction.rst.
---
...mera_calibration_and_3d_reconstruction.rst | 23 +
modules/calib3d/include/opencv2/calib3d.hpp | 5 +
modules/calib3d/src/homography_decomp.cpp | 480 ++++++++++++++++++
.../calib3d/test/test_homography_decomp.cpp | 138 +++++
4 files changed, 646 insertions(+)
create mode 100644 modules/calib3d/src/homography_decomp.cpp
create mode 100644 modules/calib3d/test/test_homography_decomp.cpp
diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
index 36af8362ff..0170904a6d 100644
--- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
+++ b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
@@ -758,6 +758,27 @@ They are
:math:`[R_2, -t]`.
By decomposing ``E``, you can only get the direction of the translation, so the function returns unit ``t``.
+decomposeHomographyMat
+--------------------------
+Decompose a homography matrix to rotation(s), translation(s) and plane normal(s).
+
+.. ocv:function:: int decomposeHomographyMat( InputArray H, InputArray K, OutputArrayOfArrays rotations, OutputArrayOfArrays translations, OutputArrayOfArrays normals)
+
+ :param H: The input homography matrix.
+
+ :param K: The input intrinsic camera calibration matrix.
+
+ :param rotations: Array of rotation matrices.
+
+ :param translations: Array of translation matrices.
+
+ :param normals: Array of plane normal matrices.
+
+This function extracts relative camera motion between two views observing a planar object from the homography ``H`` induced by the plane.
+The intrinsic camera matrix ``K`` must also be provided. The function may return up to four mathematical solution sets. At least two of the
+solutions may further be invalidated if point correspondences are available by applying positive depth constraint (all points must be in front of the camera).
+The decomposition method is described in detail in [Malis]_.
+
recoverPose
---------------
@@ -1512,3 +1533,5 @@ The function reconstructs 3-dimensional points (in homogeneous coordinates) by u
.. [Slabaugh] Slabaugh, G.G. Computing Euler angles from a rotation matrix. http://www.soi.city.ac.uk/~sbbh653/publications/euler.pdf (verified: 2013-04-15)
.. [Zhang2000] Z. Zhang. A Flexible New Technique for Camera Calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
+
+.. [Malis] Malis, E. and Vargas, M. Deeper understanding of the homography decomposition for vision-based control, Research Report 6303, INRIA (2007)
diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index 8b9b69c3aa..fd6ef8ceed 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -314,6 +314,11 @@ CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst,
double ransacThreshold = 3, double confidence = 0.99);
+CV_EXPORTS_W int decomposeHomographyMat(InputArray _H,
+ InputArray _K,
+ OutputArrayOfArrays _rotations,
+ OutputArrayOfArrays _translations,
+ OutputArrayOfArrays _normals);
class CV_EXPORTS_W StereoMatcher : public Algorithm
{
diff --git a/modules/calib3d/src/homography_decomp.cpp b/modules/calib3d/src/homography_decomp.cpp
new file mode 100644
index 0000000000..8323453cda
--- /dev/null
+++ b/modules/calib3d/src/homography_decomp.cpp
@@ -0,0 +1,480 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ // This is a homography decomposition implementation contributed to OpenCV
+ // by Samson Yilma. It implements the homography decomposition algorithm
+ // descriped in the research report:
+ // Malis, E and Vargas, M, "Deeper understanding of the homography decomposition
+ // for vision-based control", Research Report 6303, INRIA (2007)
+ //
+ // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ // By downloading, copying, installing or using the software you agree to this license.
+ // If you do not agree to this license, do not download, install,
+ // copy or use the software.
+ //
+ //
+ // License Agreement
+ // For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2014, Samson Yilma¸ (samson_yilma@yahoo.com), all rights reserved.
+ //
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ // * Redistribution's of source code must retain the above copyright notice,
+ // this list of conditions and the following disclaimer.
+ //
+ // * Redistribution's in binary form must reproduce the above copyright notice,
+ // this list of conditions and the following disclaimer in the documentation
+ // and/or other materials provided with the distribution.
+ //
+ // * The name of the copyright holders may not be used to endorse or promote products
+ // derived from this software without specific prior written permission.
+ //
+ // This software is provided by the copyright holders and contributors "as is" and
+ // any express or implied warranties, including, but not limited to, the implied
+ // warranties of merchantability and fitness for a particular purpose are disclaimed.
+ // In no event shall the Intel Corporation or contributors be liable for any direct,
+ // indirect, incidental, special, exemplary, or consequential damages
+ // (including, but not limited to, procurement of substitute goods or services;
+ // loss of use, data, or profits; or business interruption) however caused
+ // and on any theory of liability, whether in contract, strict liability,
+ // or tort (including negligence or otherwise) arising in any way out of
+ // the use of this software, even if advised of the possibility of such damage.
+ //
+ //M*/
+
+#include "precomp.hpp"
+
+namespace cv
+{
+
+namespace HomographyDecomposition
+{
+
+//struct to hold solutions of homography decomposition
+typedef struct _CameraMotion {
+ cv::Matx33d R; //!< rotation matrix
+ cv::Vec3d n; //!< normal of the plane the camera is looking at
+ cv::Vec3d t; //!< translation vector
+} CameraMotion;
+
+inline int signd(const double x)
+{
+ return ( x >= 0 ? 1 : -1 );
+}
+
+class HomographyDecomp {
+
+public:
+ HomographyDecomp() {}
+ virtual ~HomographyDecomp() {}
+ virtual void decomposeHomography(const cv::Matx33d& H, const cv::Matx33d& K,
+ std::vector& camMotions);
+ bool isRotationValid(const cv::Matx33d& R, const double epsilon=0.01);
+
+protected:
+ bool passesSameSideOfPlaneConstraint(CameraMotion& motion);
+ virtual void decompose(std::vector& camMotions) = 0;
+ const cv::Matx33d& getHnorm() const {
+ return _Hnorm;
+ }
+
+private:
+ cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K);
+ void removeScale();
+ cv::Matx33d _Hnorm;
+};
+
+class HomographyDecompZhang : public HomographyDecomp {
+
+public:
+ HomographyDecompZhang():HomographyDecomp() {}
+ virtual ~HomographyDecompZhang() {}
+
+private:
+ virtual void decompose(std::vector& camMotions);
+ bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion);
+};
+
+class HomographyDecompInria : public HomographyDecomp {
+
+public:
+ HomographyDecompInria():HomographyDecomp() {}
+ virtual ~HomographyDecompInria() {}
+
+private:
+ virtual void decompose(std::vector& camMotions);
+ double oppositeOfMinor(const cv::Matx33d& M, const int row, const int col);
+ void findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R);
+};
+
+// normalizes homography with intrinsic camera parameters
+Matx33d HomographyDecomp::normalize(const Matx33d& H, const Matx33d& K)
+{
+ return K.inv() * H * K;
+}
+
+void HomographyDecomp::removeScale()
+{
+ Mat W;
+ SVD::compute(_Hnorm, W);
+ _Hnorm = _Hnorm * (1.0/W.at(1));
+}
+
+/*! This checks that the input is a pure rotation matrix 'm'.
+ * The conditions for this are: R' * R = I and det(R) = 1 (proper rotation matrix)
+ */
+bool HomographyDecomp::isRotationValid(const Matx33d& R, const double epsilon)
+{
+ Matx33d RtR = R.t() * R;
+ Matx33d I(1,0,0, 0,1,0, 0,0,1);
+ if (norm(RtR, I, NORM_INF) > epsilon)
+ return false;
+ return (fabs(determinant(R) - 1.0) < epsilon);
+}
+
+bool HomographyDecomp::passesSameSideOfPlaneConstraint(CameraMotion& motion)
+{
+ typedef Matx Matx11d;
+ Matx31d t = Matx31d(motion.t);
+ Matx31d n = Matx31d(motion.n);
+ Matx11d proj = n.t() * motion.R.t() * t;
+ if ( (1 + proj(0, 0) ) <= 0 )
+ return false;
+ return true;
+}
+
+//!main routine to decompose homography
+void HomographyDecomp::decomposeHomography(const Matx33d& H, const cv::Matx33d& K,
+ std::vector& camMotions)
+{
+ //normalize homography matrix with intrinsic camera matrix
+ _Hnorm = normalize(H, K);
+ //remove scale of the normalized homography
+ removeScale();
+ //apply decomposition
+ decompose(camMotions);
+}
+
+/* function computes R&t from tstar, and plane normal(n) using
+ R = H * inv(I + tstar*transpose(n) );
+ t = R * tstar;
+ returns true if computed R&t is a valid solution
+ */
+bool HomographyDecompZhang::findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion)
+{
+ Matx31d tstar_m = Mat(tstar);
+ Matx31d n_m = Mat(n);
+ Matx33d temp = tstar_m * n_m.t();
+ temp(0, 0) += 1.0;
+ temp(1, 1) += 1.0;
+ temp(2, 2) += 1.0;
+ motion.R = getHnorm() * temp.inv();
+ motion.t = motion.R * tstar;
+ motion.n = n;
+ return passesSameSideOfPlaneConstraint(motion);
+}
+
+void HomographyDecompZhang::decompose(std::vector& camMotions)
+{
+ Mat W, U, Vt;
+ SVD::compute(getHnorm(), W, U, Vt);
+ double lambda1=W.at(0);
+ double lambda3=W.at(2);
+ double lambda1m3 = (lambda1-lambda3);
+ double lambda1m3_2 = lambda1m3*lambda1m3;
+ double lambda1t3 = lambda1*lambda3;
+
+ double t1 = 1.0/(2.0*lambda1t3);
+ double t2 = sqrtf(1.0+4.0*lambda1t3/lambda1m3_2);
+ double t12 = t1*t2;
+
+ double e1 = -t1 + t12; //t1*(-1.0f + t2 );
+ double e3 = -t1 - t12; //t1*(-1.0f - t2);
+ double e1_2 = e1*e1;
+ double e3_2 = e3*e3;
+
+ double nv1p = sqrtf(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0);
+ double nv3p = sqrtf(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0);
+ double v1p[3], v3p[3];
+
+ v1p[0]=Vt.at(0)*nv1p, v1p[1]=Vt.at(1)*nv1p, v1p[2]=Vt.at(2)*nv1p;
+ v3p[0]=Vt.at(6)*nv3p, v3p[1]=Vt.at(7)*nv3p, v3p[2]=Vt.at(8)*nv3p;
+
+ /*The eight solutions are
+ (A): tstar = +- (v1p - v3p)/(e1 -e3), n = +- (e1*v3p - e3*v1p)/(e1-e3)
+ (B): tstar = +- (v1p + v3p)/(e1 -e3), n = +- (e1*v3p + e3*v1p)/(e1-e3)
+ */
+ double v1pmv3p[3], v1ppv3p[3];
+ double e1v3me3v1[3], e1v3pe3v1[3];
+ double inv_e1me3 = 1.0/(e1-e3);
+
+ for(int kk=0;kk<3;++kk){
+ v1pmv3p[kk] = v1p[kk]-v3p[kk];
+ v1ppv3p[kk] = v1p[kk]+v3p[kk];
+ }
+
+ for(int kk=0; kk<3; ++kk){
+ double e1v3 = e1*v3p[kk];
+ double e3v1=e3*v1p[kk];
+ e1v3me3v1[kk] = e1v3-e3v1;
+ e1v3pe3v1[kk] = e1v3+e3v1;
+ }
+
+ Vec3d tstar_p, tstar_n;
+ Vec3d n_p, n_n;
+
+ ///Solution group A
+ for(int kk=0; kk<3; ++kk) {
+ tstar_p[kk] = v1pmv3p[kk]*inv_e1me3;
+ tstar_n[kk] = -tstar_p[kk];
+ n_p[kk] = e1v3me3v1[kk]*inv_e1me3;
+ n_n[kk] = -n_p[kk];
+ }
+
+ CameraMotion cmotion;
+ //(A) Four different combinations for solution A
+ // (i) (+, +)
+ if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (ii) (+, -)
+ if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (iii) (-, +)
+ if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (iv) (-, -)
+ if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
+ camMotions.push_back(cmotion);
+ //////////////////////////////////////////////////////////////////
+ ///Solution group B
+ for(int kk=0;kk<3;++kk){
+ tstar_p[kk] = v1ppv3p[kk]*inv_e1me3;
+ tstar_n[kk] = -tstar_p[kk];
+ n_p[kk] = e1v3pe3v1[kk]*inv_e1me3;
+ n_n[kk] = -n_p[kk];
+ }
+
+ //(B) Four different combinations for solution B
+ // (i) (+, +)
+ if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (ii) (+, -)
+ if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (iii) (-, +)
+ if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion))
+ camMotions.push_back(cmotion);
+
+ // (iv) (-, -)
+ if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion))
+ camMotions.push_back(cmotion);
+}
+
+double HomographyDecompInria::oppositeOfMinor(const Matx33d& M, const int row, const int col)
+{
+ int x1 = col == 0 ? 1 : 0;
+ int x2 = col == 2 ? 1 : 2;
+ int y1 = row == 0 ? 1 : 0;
+ int y2 = row == 2 ? 1 : 2;
+
+ return (M(y1, x2) * M(y2, x1) - M(y1, x1) * M(y2, x2));
+}
+
+//computes R = H( I - (2/v)*te_star*ne_t )
+void HomographyDecompInria::findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R)
+{
+ Matx31d tstar_m = Matx31d(tstar);
+ Matx31d n_m = Matx31d(n);
+ Matx33d I(1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0);
+
+ R = getHnorm() * (I - (2/v) * tstar_m * n_m.t() );
+}
+
+void HomographyDecompInria::decompose(std::vector& camMotions)
+{
+ const double epsilon = 0.001;
+ Matx33d S;
+
+ //S = H'H - I
+ S = getHnorm().t() * getHnorm();
+ S(0, 0) -= 1.0;
+ S(1, 1) -= 1.0;
+ S(2, 2) -= 1.0;
+
+ //check if H is rotation matrix
+ if( norm(S, NORM_INF) < epsilon) {
+ CameraMotion motion;
+ motion.R = Matx33d(getHnorm());
+ motion.t = Vec3d(0, 0, 0);
+ motion.n = Vec3d(0, 0, 0);
+ camMotions.push_back(motion);
+ return;
+ }
+
+ //! Compute nvectors
+ Vec3d npa, npb;
+
+ double M00 = oppositeOfMinor(S, 0, 0);
+ double M11 = oppositeOfMinor(S, 1, 1);
+ double M22 = oppositeOfMinor(S, 2, 2);
+
+ double rtM00 = sqrt(M00);
+ double rtM11 = sqrt(M11);
+ double rtM22 = sqrt(M22);
+
+ double M01 = oppositeOfMinor(S, 0, 1);
+ double M12 = oppositeOfMinor(S, 1, 2);
+ double M02 = oppositeOfMinor(S, 0, 2);
+
+ int e12 = signd(M12);
+ int e02 = signd(M02);
+ int e01 = signd(M01);
+
+ double nS00 = abs(S(0, 0));
+ double nS11 = abs(S(1, 1));
+ double nS22 = abs(S(2, 2));
+
+ //find max( |Sii| ), i=0, 1, 2
+ int indx = 0;
+ if(nS00 < nS11){
+ indx = 1;
+ if( nS11 < nS22 )
+ indx = 2;
+ }
+ else {
+ if(nS00 < nS22 )
+ indx = 2;
+ }
+
+ switch (indx) {
+ case 0:
+ npa[0] = S(0, 0), npb[0] = S(0, 0);
+ npa[1] = S(0, 1) + rtM22, npb[1] = S(0, 1) - rtM22;
+ npa[2] = S(0, 2) + e12 * rtM11, npb[2] = S(0, 2) - e12 * rtM11;
+ break;
+ case 1:
+ npa[0] = S(0, 1) + rtM22, npb[0] = S(0, 1) - rtM22;
+ npa[1] = S(1, 1), npb[1] = S(1, 1);
+ npa[2] = S(1, 2) - e02 * rtM00, npb[2] = S(1, 2) + e02 * rtM00;
+ break;
+ case 2:
+ npa[0] = S(0, 2) + e01 * rtM11, npb[0] = S(0, 2) - e01 * rtM11;
+ npa[1] = S(1, 2) + rtM00, npb[1] = S(1, 2) - rtM00;
+ npa[2] = S(2, 2), npb[2] = S(2, 2);
+ break;
+ default:
+ break;
+ }
+
+ double traceS = S(0, 0) + S(1, 1) + S(2, 2);
+ double v = 2.0 * sqrtf(1 + traceS - M00 - M11 - M22);
+
+ double ESii = signd(S(indx, indx)) ;
+ double r_2 = 2 + traceS + v;
+ double nt_2 = 2 + traceS - v;
+
+ double r = sqrt(r_2);
+ double n_t = sqrt(nt_2);
+
+ Vec3d na = npa / norm(npa);
+ Vec3d nb = npb / norm(npb);
+
+ double half_nt = 0.5 * n_t;
+ double esii_t_r = ESii * r;
+
+ Vec3d ta_star = half_nt * (esii_t_r * nb - n_t * na);
+ Vec3d tb_star = half_nt * (esii_t_r * na - n_t * nb);
+
+ camMotions.resize(4);
+
+ Matx33d Ra, Rb;
+ Vec3d ta, tb;
+
+ //Ra, ta, na
+ findRmatFrom_tstar_n(ta_star, na, v, Ra);
+ ta = Ra * ta_star;
+
+ camMotions[0].R = Ra;
+ camMotions[0].t = ta;
+ camMotions[0].n = na;
+
+ //Ra, -ta, -na
+ camMotions[1].R = Ra;
+ camMotions[1].t = -ta;
+ camMotions[1].n = -na;
+
+ //Rb, tb, nb
+ findRmatFrom_tstar_n(tb_star, nb, v, Rb);
+ tb = Rb * tb_star;
+
+ camMotions[2].R = Rb;
+ camMotions[2].t = tb;
+ camMotions[2].n = nb;
+
+ //Rb, -tb, -nb
+ camMotions[3].R = Rb;
+ camMotions[3].t = -tb;
+ camMotions[3].n = -nb;
+}
+
+} //namespace HomographyDecomposition
+
+int decomposeHomographyMat(InputArray _H,
+ InputArray _K,
+ OutputArrayOfArrays _rotations,
+ OutputArrayOfArrays _translations,
+ OutputArrayOfArrays _normals)
+{
+ using namespace std;
+ using namespace HomographyDecomposition;
+
+ Mat H = _H.getMat().reshape(1, 3);
+ CV_Assert(H.cols == 3 && H.rows == 3);
+
+ Mat K = _K.getMat().reshape(1, 3);
+ CV_Assert(K.cols == 3 && K.rows == 3);
+
+ auto_ptr hdecomp(new HomographyDecompInria);
+
+ vector motions;
+ hdecomp->decomposeHomography(H, K, motions);
+
+ int nsols = static_cast(motions.size());
+ int depth = CV_64F; //double precision matrices used in CameraMotion struct
+
+ if (_rotations.needed()) {
+ _rotations.create(nsols, 1, depth);
+ for (int k = 0; k < nsols; ++k ) {
+ _rotations.getMatRef(k) = Mat(motions[k].R);
+ }
+ }
+
+ if (_translations.needed()) {
+ _translations.create(nsols, 1, depth);
+ for (int k = 0; k < nsols; ++k ) {
+ _translations.getMatRef(k) = Mat(motions[k].t);
+ }
+ }
+
+ if (_normals.needed()) {
+ _normals.create(nsols, 1, depth);
+ for (int k = 0; k < nsols; ++k ) {
+ _normals.getMatRef(k) = Mat(motions[k].n);
+ }
+ }
+
+ return nsols;
+}
+
+} //namespace cv
diff --git a/modules/calib3d/test/test_homography_decomp.cpp b/modules/calib3d/test/test_homography_decomp.cpp
new file mode 100644
index 0000000000..dbe62c0c83
--- /dev/null
+++ b/modules/calib3d/test/test_homography_decomp.cpp
@@ -0,0 +1,138 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ // This is a test file for the function decomposeHomography contributed to OpenCV
+ // by Samson Yilma.
+ //
+ // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ // By downloading, copying, installing or using the software you agree to this license.
+ // If you do not agree to this license, do not download, install,
+ // copy or use the software.
+ //
+ //
+ // License Agreement
+ // For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2014, Samson Yilma¸ (samson_yilma@yahoo.com), all rights reserved.
+ //
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ // * Redistribution's of source code must retain the above copyright notice,
+ // this list of conditions and the following disclaimer.
+ //
+ // * Redistribution's in binary form must reproduce the above copyright notice,
+ // this list of conditions and the following disclaimer in the documentation
+ // and/or other materials provided with the distribution.
+ //
+ // * The name of the copyright holders may not be used to endorse or promote products
+ // derived from this software without specific prior written permission.
+ //
+ // This software is provided by the copyright holders and contributors "as is" and
+ // any express or implied warranties, including, but not limited to, the implied
+ // warranties of merchantability and fitness for a particular purpose are disclaimed.
+ // In no event shall the Intel Corporation or contributors be liable for any direct,
+ // indirect, incidental, special, exemplary, or consequential damages
+ // (including, but not limited to, procurement of substitute goods or services;
+ // loss of use, data, or profits; or business interruption) however caused
+ // and on any theory of liability, whether in contract, strict liability,
+ // or tort (including negligence or otherwise) arising in any way out of
+ // the use of this software, even if advised of the possibility of such damage.
+ //
+ //M*/
+
+#include "test_precomp.hpp"
+#include "opencv2/calib3d.hpp"
+#include
+
+using namespace cv;
+using namespace std;
+
+class CV_HomographyDecompTest: public cvtest::BaseTest {
+
+public:
+ CV_HomographyDecompTest()
+ {
+ buildTestDataSet();
+ }
+
+protected:
+ void run(int)
+ {
+ vector rotations;
+ vector translations;
+ vector normals;
+
+ decomposeHomographyMat(_H, _K, rotations, translations, normals);
+
+ //there should be at least 1 solution
+ ASSERT_GT(rotations.size(), 0);
+ ASSERT_GT(translations.size(), 0);
+ ASSERT_GT(normals.size(), 0);
+
+ ASSERT_EQ(rotations.size(), normals.size());
+ ASSERT_EQ(translations.size(), normals.size());
+
+ ASSERT_TRUE(containsValidMotion(rotations, translations, normals));
+
+ decomposeHomographyMat(_H, _K, rotations, noArray(), noArray());
+ ASSERT_GT(rotations.size(), 0);
+ }
+
+private:
+
+ void buildTestDataSet()
+ {
+ _K = Matx33d(640, 0.0, 320,
+ 0, 640, 240,
+ 0, 0, 1);
+
+ _H = Matx33d(2.649157564634028, 4.583875997496426, 70.694447785121326,
+ -1.072756858861583, 3.533262150437228, 1513.656999614321649,
+ 0.001303887589576, 0.003042206876298, 1.000000000000000
+ );
+
+ //expected solution for the given homography and intrinsic matrices
+ _R = Matx33d(0.43307983549125, 0.545749113549648, -0.717356090899523,
+ -0.85630229674426, 0.497582023798831, -0.138414255706431,
+ 0.281404038139784, 0.67421809131173, 0.682818960388909);
+
+ _t = Vec3d(1.826751712278038, 1.264718492450820, 0.195080809998819);
+ _n = Vec3d(0.244875830334816, 0.480857890778889, 0.841909446789566);
+ }
+
+ bool containsValidMotion(std::vector& rotations,
+ std::vector& translations,
+ std::vector& normals
+ )
+ {
+ double max_error = 1.0e-3;
+
+ vector::iterator riter = rotations.begin();
+ vector::iterator titer = translations.begin();
+ vector::iterator niter = normals.begin();
+
+ for (;
+ riter != rotations.end() && titer != translations.end() && niter != normals.end();
+ ++riter, ++titer, ++niter) {
+
+ double rdist = norm(*riter, _R, NORM_INF);
+ double tdist = norm(*titer, _t, NORM_INF);
+ double ndist = norm(*niter, _n, NORM_INF);
+
+ if ( rdist < max_error
+ && tdist < max_error
+ && ndist < max_error )
+ return true;
+ }
+
+ return false;
+ }
+
+ Matx33d _R, _K, _H;
+ Vec3d _t, _n;
+};
+
+TEST(Calib3d_DecomposeHomography, regression) { CV_HomographyDecompTest test; test.safe_run(); }
From de55126b69affe81c25a368a4c111cf3f56ce4a6 Mon Sep 17 00:00:00 2001
From: Samson Yilma
Date: Sun, 27 Apr 2014 13:02:36 -0400
Subject: [PATCH 006/189] Fixed warnings and parameter name mismatches, added
#include needed in some platforms.
---
modules/calib3d/include/opencv2/calib3d.hpp | 10 +++++-----
modules/calib3d/src/homography_decomp.cpp | 9 +++++----
modules/calib3d/test/test_homography_decomp.cpp | 8 ++++----
3 files changed, 14 insertions(+), 13 deletions(-)
diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index fd6ef8ceed..b18c00f8d2 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -314,11 +314,11 @@ CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst,
double ransacThreshold = 3, double confidence = 0.99);
-CV_EXPORTS_W int decomposeHomographyMat(InputArray _H,
- InputArray _K,
- OutputArrayOfArrays _rotations,
- OutputArrayOfArrays _translations,
- OutputArrayOfArrays _normals);
+CV_EXPORTS_W int decomposeHomographyMat(InputArray H,
+ InputArray K,
+ OutputArrayOfArrays rotations,
+ OutputArrayOfArrays translations,
+ OutputArrayOfArrays normals);
class CV_EXPORTS_W StereoMatcher : public Algorithm
{
diff --git a/modules/calib3d/src/homography_decomp.cpp b/modules/calib3d/src/homography_decomp.cpp
index 8323453cda..7a5c31b43c 100644
--- a/modules/calib3d/src/homography_decomp.cpp
+++ b/modules/calib3d/src/homography_decomp.cpp
@@ -47,6 +47,7 @@
//M*/
#include "precomp.hpp"
+#include
namespace cv
{
@@ -189,7 +190,7 @@ void HomographyDecompZhang::decompose(std::vector& camMotions)
double lambda1t3 = lambda1*lambda3;
double t1 = 1.0/(2.0*lambda1t3);
- double t2 = sqrtf(1.0+4.0*lambda1t3/lambda1m3_2);
+ double t2 = sqrt(1.0+4.0*lambda1t3/lambda1m3_2);
double t12 = t1*t2;
double e1 = -t1 + t12; //t1*(-1.0f + t2 );
@@ -197,8 +198,8 @@ void HomographyDecompZhang::decompose(std::vector& camMotions)
double e1_2 = e1*e1;
double e3_2 = e3*e3;
- double nv1p = sqrtf(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0);
- double nv3p = sqrtf(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0);
+ double nv1p = sqrt(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0);
+ double nv3p = sqrt(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0);
double v1p[3], v3p[3];
v1p[0]=Vt.at(0)*nv1p, v1p[1]=Vt.at(1)*nv1p, v1p[2]=Vt.at(2)*nv1p;
@@ -378,7 +379,7 @@ void HomographyDecompInria::decompose(std::vector& camMotions)
}
double traceS = S(0, 0) + S(1, 1) + S(2, 2);
- double v = 2.0 * sqrtf(1 + traceS - M00 - M11 - M22);
+ double v = 2.0 * sqrt(1 + traceS - M00 - M11 - M22);
double ESii = signd(S(indx, indx)) ;
double r_2 = 2 + traceS + v;
diff --git a/modules/calib3d/test/test_homography_decomp.cpp b/modules/calib3d/test/test_homography_decomp.cpp
index dbe62c0c83..7e1c8ea503 100644
--- a/modules/calib3d/test/test_homography_decomp.cpp
+++ b/modules/calib3d/test/test_homography_decomp.cpp
@@ -68,9 +68,9 @@ protected:
decomposeHomographyMat(_H, _K, rotations, translations, normals);
//there should be at least 1 solution
- ASSERT_GT(rotations.size(), 0);
- ASSERT_GT(translations.size(), 0);
- ASSERT_GT(normals.size(), 0);
+ ASSERT_GT(static_cast(rotations.size()), 0);
+ ASSERT_GT(static_cast(translations.size()), 0);
+ ASSERT_GT(static_cast(normals.size()), 0);
ASSERT_EQ(rotations.size(), normals.size());
ASSERT_EQ(translations.size(), normals.size());
@@ -78,7 +78,7 @@ protected:
ASSERT_TRUE(containsValidMotion(rotations, translations, normals));
decomposeHomographyMat(_H, _K, rotations, noArray(), noArray());
- ASSERT_GT(rotations.size(), 0);
+ ASSERT_GT(static_cast(rotations.size()), 0);
}
private:
From 05ee15f10881265f1e930cc65a83951844f7828a Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Mon, 28 Apr 2014 12:01:27 +0400
Subject: [PATCH 007/189] Added FisheyeTest
---
modules/calib3d/test/test_fisheye.cpp | 329 ++++++++++++++------------
1 file changed, 173 insertions(+), 156 deletions(-)
diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp
index b2f47987cb..e7d4b311c7 100644
--- a/modules/calib3d/test/test_fisheye.cpp
+++ b/modules/calib3d/test/test_fisheye.cpp
@@ -2,138 +2,49 @@
#include
#include
-#define DEF_PARAM_TEST(name, ...) typedef ::perf::TestBaseWithParam< std::tr1::tuple< __VA_ARGS__ > > name
-#define PARAM_TEST_CASE(name, ...) struct name : testing::TestWithParam< std::tr1::tuple< __VA_ARGS__ > >
-
-/// Change this parameter via CMake: cmake -DDATASETS_REPOSITORY_FOLDER=
-//const static std::string datasets_repository_path = "DATASETS_REPOSITORY_FOLDER";
-const static std::string datasets_repository_path = "/home/krylov/data";
-
-namespace FishEye
-{
- const static cv::Size imageSize(1280, 800);
-
- const static cv::Matx33d K(558.478087865323, 0, 620.458515360843,
- 0, 560.506767351568, 381.939424848348,
- 0, 0, 1);
-
- const static cv::Vec4d D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
-
- const static cv::Matx33d R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
- -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
- -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
-
- const static cv::Vec3d T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
-}
-
-namespace{
-std::string combine(const std::string& _item1, const std::string& _item2)
-{
- std::string item1 = _item1, item2 = _item2;
- std::replace(item1.begin(), item1.end(), '\\', '/');
- std::replace(item2.begin(), item2.end(), '\\', '/');
-
- if (item1.empty())
- return item2;
-
- if (item2.empty())
- return item1;
-
- char last = item1[item1.size()-1];
- return item1 + (last != '/' ? "/" : "") + item2;
-}
-
-std::string combine_format(const std::string& item1, const std::string& item2, ...)
-{
- std::string fmt = combine(item1, item2);
- char buffer[1 << 16];
- va_list args;
- va_start( args, item2 );
- vsprintf( buffer, fmt.c_str(), args );
- va_end( args );
- return std::string(buffer);
-}
-
-void readPoins(std::vector >& objectPoints,
- std::vector >& imagePoints,
- const std::string& path, const int n_images, const int n_points)
-{
- objectPoints.resize(n_images);
- imagePoints.resize(n_images);
-
- std::vector image(n_points);
- std::vector object(n_points);
-
- std::ifstream ipStream;
- std::ifstream opStream;
-
- for (int image_idx = 0; image_idx < n_images; image_idx++)
- {
- std::stringstream ss;
- ss << image_idx;
- std::string idxStr = ss.str();
-
- ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
- opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
- CV_Assert(ipStream.is_open() && opStream.is_open());
-
- for (int point_idx = 0; point_idx < n_points; point_idx++)
- {
- double x, y, z;
- char delim;
- ipStream >> x >> delim >> y;
- image[point_idx] = cv::Point2d(x, y);
- opStream >> x >> delim >> y >> delim >> z;
- object[point_idx] = cv::Point3d(x, y, z);
- }
- ipStream.close();
- opStream.close();
-
- imagePoints[image_idx] = image;
- objectPoints[image_idx] = object;
+class FisheyeTest : public ::testing::Test {
+
+protected:
+ const static cv::Size imageSize;
+ const static cv::Matx33d K;
+ const static cv::Vec4d D;
+ const static cv::Matx33d R;
+ const static cv::Vec3d T;
+ std::string datasets_repository_path;
+
+ virtual void SetUp() {
+ datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cameracalibration/fisheye");
}
-}
-void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
- cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q)
-{
- cv::FileStorage fs(file, cv::FileStorage::READ);
- CV_Assert(fs.isOpened());
+protected:
+ std::string combine(const std::string& _item1, const std::string& _item2);
- cv::Mat R, T, R1, R2, P1, P2, Q;
- fs["R"] >> R; fs["T"] >> T; fs["R1"] >> R1; fs["R2"] >> R2; fs["P1"] >> P1; fs["P2"] >> P2; fs["Q"] >> Q;
- if (_R.needed()) R.copyTo(_R); if(_T.needed()) T.copyTo(_T); if (_R1.needed()) R1.copyTo(_R1); if (_R2.needed()) R2.copyTo(_R2);
- if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q);
-}
+ std::string combine_format(const std::string& item1, const std::string& item2, ...);
-cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r)
-{
- CV_Assert(l.type() == r.type() && l.size() == r.size());
- cv::Mat merged(l.rows, l.cols * 2, l.type());
- cv::Mat lpart = merged.colRange(0, l.cols);
- cv::Mat rpart = merged.colRange(l.cols, merged.cols);
- l.copyTo(lpart);
- r.copyTo(rpart);
+ void readPoins(std::vector >& objectPoints,
+ std::vector >& imagePoints,
+ const std::string& path, const int n_images, const int n_points);
- for(int i = 0; i < l.rows; i+=20)
- cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0));
+ void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
+ cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q);
- return merged;
-}
+ cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r);
+};
-}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// TESTS::
-TEST(FisheyeTest, projectPoints)
+TEST_F(FisheyeTest, projectPoints)
{
- double cols = FishEye::imageSize.width,
- rows = FishEye::imageSize.height;
+ double cols = this->imageSize.width,
+ rows = this->imageSize.height;
const int N = 20;
cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2;
undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3));
cv::Vec2d* pts = distorted0.ptr();
- cv::Vec2d c(FishEye::K(0, 2), FishEye::K(1, 2));
+ cv::Vec2d c(this->K(0, 2), this->K(1, 2));
for(int y = 0, k = 0; y < N; ++y)
for(int x = 0; x < N; ++x)
{
@@ -141,29 +52,27 @@ TEST(FisheyeTest, projectPoints)
pts[k++] = (point - c) * 0.85 + c;
}
- cv::Fisheye::undistortPoints(distorted0, undist1, FishEye::K, FishEye::D);
+ cv::Fisheye::undistortPoints(distorted0, undist1, this->K, this->D);
cv::Vec2d* u1 = undist1.ptr();
cv::Vec3d* u2 = undist2.ptr();
for(int i = 0; i < (int)distorted0.total(); ++i)
u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0);
- cv::Fisheye::distortPoints(undist1, distorted1, FishEye::K, FishEye::D);
- cv::Fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), FishEye::K, FishEye::D);
+ cv::Fisheye::distortPoints(undist1, distorted1, this->K, this->D);
+ cv::Fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D);
- EXPECT_MAT_NEAR(distorted0, distorted1, 1e-5);
- EXPECT_MAT_NEAR(distorted0, distorted2, 1e-5);
+ EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10);
+ EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10);
}
-TEST(FisheyeTest, undistortImage)
+TEST_F(FisheyeTest, undistortImage)
{
- cv::Matx33d K = FishEye::K;
- cv::Mat D = cv::Mat(FishEye::D);
+ cv::Matx33d K = this->K;
+ cv::Mat D = cv::Mat(this->D);
std::string file = combine(datasets_repository_path, "image000001.png");
-
cv::Matx33d newK = K;
cv::Mat distorted = cv::imread(file), undistorted;
-
{
newK(0, 0) = 100;
newK(1, 1) = 100;
@@ -172,7 +81,7 @@ TEST(FisheyeTest, undistortImage)
if (correct.empty())
CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/new_f_100.png"), undistorted));
else
- EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
}
{
double balance = 1.0;
@@ -182,7 +91,7 @@ TEST(FisheyeTest, undistortImage)
if (correct.empty())
CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_1.0.png"), undistorted));
else
- EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
}
{
@@ -193,13 +102,13 @@ TEST(FisheyeTest, undistortImage)
if (correct.empty())
CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_0.0.png"), undistorted));
else
- EXPECT_MAT_NEAR(correct, undistorted, 1e-15);
+ EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
}
cv::waitKey();
}
-TEST(FisheyeTest, jacobians)
+TEST_F(FisheyeTest, jacobians)
{
int n = 10;
cv::Mat X(1, n, CV_64FC3);
@@ -245,7 +154,7 @@ TEST(FisheyeTest, jacobians)
cv::Mat T2 = T + dT;
cv::Fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on om:
cv::Mat dom(3, 1, CV_64FC1);
@@ -254,7 +163,7 @@ TEST(FisheyeTest, jacobians)
cv::Mat om2 = om + dom;
cv::Fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on f:
cv::Mat df(2, 1, CV_64FC1);
@@ -263,7 +172,7 @@ TEST(FisheyeTest, jacobians)
cv::Matx33d K2 = K + cv::Matx33d(df.at(0), df.at(0) * alpha, 0, 0, df.at(1), 0, 0, 0, 0);
cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on c:
cv::Mat dc(2, 1, CV_64FC1);
@@ -272,7 +181,7 @@ TEST(FisheyeTest, jacobians)
K2 = K + cv::Matx33d(0, 0, dc.at(0), 0, 0, dc.at(1), 0, 0, 0);
cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on k:
cv::Mat dk(4, 1, CV_64FC1);
@@ -281,7 +190,7 @@ TEST(FisheyeTest, jacobians)
cv::Mat k2 = k + dk;
cv::Fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on alpha:
cv::Mat dalpha(1, 1, CV_64FC1);
@@ -291,10 +200,10 @@ TEST(FisheyeTest, jacobians)
K2 = K + cv::Matx33d(0, f.at(0) * dalpha.at(0), 0, 0, 0, 0, 0, 0, 0);
cv::Fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray());
xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
- CV_Assert (cv::norm(x2 - xpred) < 1e-12);
+ CV_Assert (cv::norm(x2 - xpred) < 1e-10);
}
-TEST(FisheyeTest, Calibration)
+TEST_F(FisheyeTest, Calibration)
{
const int n_images = 34;
const int n_points = 48;
@@ -316,11 +225,11 @@ TEST(FisheyeTest, Calibration)
cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
- EXPECT_MAT_NEAR(K, FishEye::K, 1e-11);
- EXPECT_MAT_NEAR(D, FishEye::D, 1e-12);
+ EXPECT_MAT_NEAR(K, this->K, 1e-10);
+ EXPECT_MAT_NEAR(D, this->D, 1e-10);
}
-TEST(FisheyeTest, Homography)
+TEST_F(FisheyeTest, Homography)
{
const int n_images = 1;
const int n_points = 48;
@@ -368,10 +277,10 @@ TEST(FisheyeTest, Homography)
std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
- EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-16);
+ EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
}
-TEST(TestFisheye, EtimateUncertainties)
+TEST_F(FisheyeTest, EtimateUncertainties)
{
const int n_images = 34;
const int n_points = 48;
@@ -393,7 +302,7 @@ TEST(TestFisheye, EtimateUncertainties)
std::vector tvec;
cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
- cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
+ rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));
cv::internal::IntrinsicParams param, errors;
cv::Vec2d err_std;
@@ -410,24 +319,24 @@ TEST(TestFisheye, EtimateUncertainties)
cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec,
errors, err_std, thresh_cond, check_cond, rms);
- EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-14);
- EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-15);
- EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-15);
- EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-15);
- CV_Assert(abs(rms - 0.263782587133546) < 1e-15);
+ EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10);
+ EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10);
+ EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10);
+ EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
+ CV_Assert(abs(rms - 0.263782587133546) < 1e-10);
CV_Assert(errors.alpha == 0);
}
-TEST(FisheyeTest, rectify)
+TEST_F(FisheyeTest, rectify)
{
const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
- cv::Size calibration_size = FishEye::imageSize, requested_size = calibration_size;
- cv::Matx33d K1 = FishEye::K, K2 = K1;
- cv::Mat D1 = cv::Mat(FishEye::D), D2 = D1;
+ cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
+ cv::Matx33d K1 = this->K, K2 = K1;
+ cv::Mat D1 = cv::Mat(this->D), D2 = D1;
- cv::Vec3d T = FishEye::T;
- cv::Matx33d R = FishEye::R;
+ cv::Vec3d T = this->T;
+ cv::Matx33d R = this->R;
double balance = 0.0, fov_scale = 1.1;
cv::Mat R1, R2, P1, P2, Q;
@@ -462,11 +371,119 @@ TEST(FisheyeTest, rectify)
if (correct.empty())
cv::imwrite(combine_format(folder, "test_rectify/rectification_AB_%03d.png", i), rectification);
else
- EXPECT_MAT_NEAR(correct, rectification, 1e-15);
+ EXPECT_MAT_NEAR(correct, rectification, 1e-10);
}
}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// FisheyeTest::
+
+const cv::Size FisheyeTest::imageSize(1280, 800);
+
+const cv::Matx33d FisheyeTest::K(558.478087865323, 0, 620.458515360843,
+ 0, 560.506767351568, 381.939424848348,
+ 0, 0, 1);
+
+const cv::Vec4d FisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
+
+const cv::Matx33d FisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
+ -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
+ -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
+
+const cv::Vec3d FisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
+
+
+std::string FisheyeTest::combine(const std::string& _item1, const std::string& _item2)
+{
+ std::string item1 = _item1, item2 = _item2;
+ std::replace(item1.begin(), item1.end(), '\\', '/');
+ std::replace(item2.begin(), item2.end(), '\\', '/');
+
+ if (item1.empty())
+ return item2;
+ if (item2.empty())
+ return item1;
+ char last = item1[item1.size()-1];
+ return item1 + (last != '/' ? "/" : "") + item2;
+}
+std::string FisheyeTest::combine_format(const std::string& item1, const std::string& item2, ...)
+{
+ std::string fmt = combine(item1, item2);
+ char buffer[1 << 16];
+ va_list args;
+ va_start( args, item2 );
+ vsprintf( buffer, fmt.c_str(), args );
+ va_end( args );
+ return std::string(buffer);
+}
+
+void FisheyeTest::readPoins(std::vector >& objectPoints,
+ std::vector >& imagePoints,
+ const std::string& path, const int n_images, const int n_points)
+{
+ objectPoints.resize(n_images);
+ imagePoints.resize(n_images);
+
+ std::vector image(n_points);
+ std::vector object(n_points);
+
+ std::ifstream ipStream;
+ std::ifstream opStream;
+
+ for (int image_idx = 0; image_idx < n_images; image_idx++)
+ {
+ std::stringstream ss;
+ ss << image_idx;
+ std::string idxStr = ss.str();
+
+ ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
+ opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
+ CV_Assert(ipStream.is_open() && opStream.is_open());
+
+ for (int point_idx = 0; point_idx < n_points; point_idx++)
+ {
+ double x, y, z;
+ char delim;
+ ipStream >> x >> delim >> y;
+ image[point_idx] = cv::Point2d(x, y);
+ opStream >> x >> delim >> y >> delim >> z;
+ object[point_idx] = cv::Point3d(x, y, z);
+ }
+ ipStream.close();
+ opStream.close();
+
+ imagePoints[image_idx] = image;
+ objectPoints[image_idx] = object;
+ }
+}
+
+void FisheyeTest::readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
+ cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q)
+{
+ cv::FileStorage fs(file, cv::FileStorage::READ);
+ CV_Assert(fs.isOpened());
+
+ cv::Mat R, T, R1, R2, P1, P2, Q;
+ fs["R"] >> R; fs["T"] >> T; fs["R1"] >> R1; fs["R2"] >> R2; fs["P1"] >> P1; fs["P2"] >> P2; fs["Q"] >> Q;
+ if (_R.needed()) R.copyTo(_R); if(_T.needed()) T.copyTo(_T); if (_R1.needed()) R1.copyTo(_R1); if (_R2.needed()) R2.copyTo(_R2);
+ if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q);
+}
+
+cv::Mat FisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r)
+{
+ CV_Assert(l.type() == r.type() && l.size() == r.size());
+ cv::Mat merged(l.rows, l.cols * 2, l.type());
+ cv::Mat lpart = merged.colRange(0, l.cols);
+ cv::Mat rpart = merged.colRange(l.cols, merged.cols);
+ l.copyTo(lpart);
+ r.copyTo(rpart);
+
+ for(int i = 0; i < l.rows; i+=20)
+ cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0));
+
+ return merged;
+}
From f0f741b796a8c1eaa9217e51d354a31e40b9107a Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Mon, 28 Apr 2014 15:27:30 +0400
Subject: [PATCH 008/189] Added documentation
---
...mera_calibration_and_3d_reconstruction.rst | 309 ++++++++++++++++++
.../include/opencv2/calib3d/calib3d.hpp | 2 +-
2 files changed, 310 insertions(+), 1 deletion(-)
diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
index 37159b016b..3d19b72895 100644
--- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
+++ b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
@@ -1487,6 +1487,315 @@ The function reconstructs 3-dimensional points (in homogeneous coordinates) by u
:ocv:func:`reprojectImageTo3D`
+Fisheye
+----------
+
+.. ocv:class:: Fisheye
+
+The methods in this class use a so-called fisheye camera model. ::
+
+ class Fisheye
+ {
+ public:
+
+ //! projects 3D points using fisheye model
+ static void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
+ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
+
+ //! projects points using fisheye model
+ static void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec,
+ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
+
+ //! distorts 2D points using fisheye model
+ static void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0);
+
+ //! undistorts 2D points using fisheye model
+ static void undistortPoints(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray());
+
+ //! computing undistortion and rectification maps for image transform by cv::remap()
+ //! If D is empty zero distortion is used, if R or P is empty identity matrixes are used
+ static void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
+ const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);
+
+ //! undistorts image, optionally changes resolution and camera matrix. If Knew zero identity matrix is used
+ static void undistortImage(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size());
+
+ //! estimates new camera matrix for undistortion or rectification
+ static void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
+ OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0);
+
+ //! stereo rectification for fisheye camera model
+ static void stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const cv::Size& imageSize,
+ InputArray rotaion, InputArray tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q,
+ int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, const Size& newImageSize = Size(), Rect* roi1 = 0, Rect* roi2 = 0 );
+
+ //! performs camera calibaration
+ static double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
+ InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0,
+ TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
+
+ //! stereo rectification estimation
+ static void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
+ OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
+ double balance = 0.0, double fov_scale = 1.0);
+
+ ...
+ };
+
+
+Definitions:
+Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
+The coordinate vector of P in the camera reference frame is:
+
+.. class:: center
+.. math::
+
+ Xc = R X + T
+
+where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
+call x, y and z the 3 coordinates of Xc:
+
+.. class:: center
+.. math::
+ x = Xc_1 \\
+ y = Xc_2 \\
+ z = Xc_3
+
+The pinehole projection coordinates of P is [a; b] where
+
+.. class:: center
+.. math::
+
+ a = x / z \ and \ b = y / z \\
+ r^2 = a^2 + b^2 \\
+ \theta = atan(r)
+
+Fisheye distortion:
+
+.. class:: center
+.. math::
+
+ \theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)
+
+The distorted point coordinates are [x'; y'] where
+
+.. class:: center
+.. math::
+
+ x' = (\theta_d / r) x \\
+ y' = (\theta_d / r) y
+
+Finally, convertion into pixel coordinates: The final pixel coordinates vector [u; v] where:
+
+.. class:: center
+.. math::
+
+ u = f_x (x' + \alpha y') + c_x \\
+ v = f_y yy + c_y
+
+Fisheye::projectPoints
+---------------------------
+Projects points using fisheye model
+
+.. ocv:function:: void Fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray())
+
+.. ocv:function:: void Fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec,
+ InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray())
+
+ :param objectPoints: Array of object points, 1xN/Nx1 3-channel (or ``vector`` ), where N is the number of points in the view.
+
+ :param rvec: Rotation vector. See :ocv:func:`Rodrigues` for details.
+
+ :param tvec: Translation vector.
+
+ :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}` .
+
+ :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param alpha: The skew coefficient.
+
+ :param imagePoints: Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or ``vector`` .
+
+ :param jacobian: Optional output 2Nx15 jacobian matrix of derivatives of image points with respect to components of the focal lengths, coordinates of the principal point, distortion coefficients, rotation vector, translation vector, and the skew. In the old interface different components of the jacobian are returned via different output parameters.
+
+The function computes projections of 3D
+points to the image plane given intrinsic and extrinsic camera
+parameters. Optionally, the function computes Jacobians - matrices
+of partial derivatives of image points coordinates (as functions of all the
+input parameters) with respect to the particular parameters, intrinsic and/or
+extrinsic.
+
+Fisheye::distortPoints
+-------------------------
+Distorts 2D points using fisheye model.
+
+.. ocv:function:: void Fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0)
+
+ :param undistorted: Array of object points, 1xN/Nx1 2-channel (or ``vector`` ), where N is the number of points in the view.
+
+ :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`.
+
+ :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param alpha: The skew coefficient.
+
+ :param distorted: Output array of image points, 1xN/Nx1 2-channel, or ``vector`` .
+
+Fisheye::undistortPoints
+-----------------------------
+Undistorts 2D points using fisheye model
+
+.. ocv:function:: void Fisheye::undistortPoints(InputArray distorted, OutputArray undistorted,
+ InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray())
+
+ :param distorted: Array of object points, 1xN/Nx1 2-channel (or ``vector`` ), where N is the number of points in the view.
+
+ :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`.
+
+ :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel
+
+ :param P: New camera matrix (3x3) or new projection matrix (3x4)
+
+ :param undistorted: Output array of image points, 1xN/Nx1 2-channel, or ``vector`` .
+
+
+Fisheye::initUndistortRectifyMap
+-------------------------------------
+Computes undistortion and rectification maps for image transform by cv::remap(). If D is empty zero distortion is used, if R or P is empty identity matrixes are used.
+
+.. ocv:function:: void Fisheye::initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
+ const cv::Size& size, int m1type, OutputArray map1, OutputArray map2)
+
+ :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`.
+
+ :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel
+
+ :param P: New camera matrix (3x3) or new projection matrix (3x4)
+
+ :param size: Undistorted image size.
+
+ :param m1type: Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() for details.
+
+ :param map1: The first output map.
+
+ :param map2: The second output map.
+
+
+Fisheye::estimateNewCameraMatrixForUndistortRectify
+----------------------------------------------------------
+Estimates new camera matrix for undistortion or rectification.
+
+.. ocv:function:: void Fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
+ OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0);
+
+ :param K: Camera matrix :math:`K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}`.
+
+ :param D: Input vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param R: Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel
+
+ :param P: New camera matrix (3x3) or new projection matrix (3x4)
+
+ :param new_size: New size
+
+ :param balance: Balance.
+
+ :param fov_scale: Field of View scale.
+
+Fisheye::stereoRectify
+------------------------------
+Stereo rectification for fisheye camera model
+
+.. ocv:function:: void Fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const cv::Size& imageSize,
+ InputArray rotaion, InputArray tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q,
+ int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, const Size& newImageSize = Size(), Rect* roi1 = 0, Rect* roi2 = 0 )
+
+.. ocv:function:: void Fisheye::stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
+ OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
+ double balance = 0.0, double fov_scale = 1.0)
+
+ :param K1: First camera matrix.
+
+ :param K2: Second camera matrix.
+
+ :param D1: First camera distortion parameters.
+
+ :param D2: Second camera distortion parameters.
+
+ :param imageSize: Size of the image used for stereo calibration.
+
+ :param rotation: Rotation matrix between the coordinate systems of the first and the second cameras.
+
+ :param tvec: Translation vector between coordinate systems of the cameras.
+
+ :param R1: Output 3x3 rectification transform (rotation matrix) for the first camera.
+
+ :param R2: Output 3x3 rectification transform (rotation matrix) for the second camera.
+
+ :param P1: Output 3x4 projection matrix in the new (rectified) coordinate systems for the first camera.
+
+ :param P2: Output 3x4 projection matrix in the new (rectified) coordinate systems for the second camera.
+
+ :param Q: Output :math:`4 \times 4` disparity-to-depth mapping matrix (see :ocv:func:`reprojectImageTo3D` ).
+
+ :param flags: Operation flags that may be zero or ``CV_CALIB_ZERO_DISPARITY`` . If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. And if the flag is not set, the function may still shift the images in the horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the useful image area.
+
+ :param alpha: Free scaling parameter. If it is -1 or absent, the function performs the default scaling. Otherwise, the parameter should be between 0 and 1. ``alpha=0`` means that the rectified images are zoomed and shifted so that only valid pixels are visible (no black areas after rectification). ``alpha=1`` means that the rectified image is decimated and shifted so that all the pixels from the original images from the cameras are retained in the rectified images (no source image pixels are lost). Obviously, any intermediate value yields an intermediate result between those two extreme cases.
+
+ :param newImageSize: New image resolution after rectification. The same size should be passed to :ocv:func:`initUndistortRectifyMap` (see the ``stereo_calib.cpp`` sample in OpenCV samples directory). When (0,0) is passed (default), it is set to the original ``imageSize`` . Setting it to larger value can help you preserve details in the original image, especially when there is a big radial distortion.
+
+ :param roi1: Optional output rectangles inside the rectified images where all the pixels are valid. If ``alpha=0`` , the ROIs cover the whole images. Otherwise, they are likely to be smaller (see the picture below).
+
+ :param roi2: Optional output rectangles inside the rectified images where all the pixels are valid. If ``alpha=0`` , the ROIs cover the whole images. Otherwise, they are likely to be smaller (see the picture below).
+
+ :param balance: Balance.
+
+ :param fov_scale: Field of View scale.
+
+
+
+Fisheye::calibrate
+----------------------------
+Performs camera calibaration
+
+.. ocv:function:: double Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
+ InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0,
+ TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON))
+
+ :param objectPoints: vector of vectors of calibration pattern points in the calibration pattern coordinate space. The outer vector contains as many elements as the number of the pattern views. If the same calibration pattern is shown in each view and it is fully visible, all the vectors will be the same. Although, it is possible to use partially occluded patterns, or even different patterns in different views. Then, the vectors will be different. The points are 3D, but since they are in a pattern coordinate system, then, if the rig is planar, it may make sense to put the model to a XY coordinate plane so that Z-coordinate of each input object point is 0.
+
+ :param imagePoints: vector of vectors of the projections of calibration pattern points. ``imagePoints.size()`` and ``objectPoints.size()`` and ``imagePoints[i].size()`` must be equal to ``objectPoints[i].size()`` for each ``i``.
+
+ :param image_size: Size of the image used only to initialize the intrinsic camera matrix.
+
+ :param K: Output 3x3 floating-point camera matrix :math:`A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}` . If ``Fisheye::CALIB_USE_INTRINSIC_GUESS``/ is specified, some or all of ``fx, fy, cx, cy`` must be initialized before calling the function.
+
+ :param D: Output vector of distortion coefficients :math:`(k_1, k_2, k_3, k_4)`.
+
+ :param rvecs: Output vector of rotation vectors (see :ocv:func:`Rodrigues` ) estimated for each pattern view. That is, each k-th rotation vector together with the corresponding k-th translation vector (see the next output parameter description) brings the calibration pattern from the model coordinate space (in which object points are specified) to the world coordinate space, that is, a real position of the calibration pattern in the k-th pattern view (k=0.. *M* -1).
+
+ :param tvecs: Output vector of translation vectors estimated for each pattern view.
+
+ :param flags: Different flags that may be zero or a combination of the following values:
+
+ * **Fisheye::CALIB_USE_INTRINSIC_GUESS** ``cameraMatrix`` contains valid initial values of ``fx, fy, cx, cy`` that are optimized further. Otherwise, ``(cx, cy)`` is initially set to the image center ( ``imageSize`` is used), and focal distances are computed in a least-squares fashion.
+
+ * **Fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration of intrinsic optimization.
+
+ * **Fisheye::CALIB_CHECK_COND** The functions will check validity of condition number.
+
+ * **Fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero.
+
+ * **Fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay zero.
+
+ :param criteria: Termination criteria for the iterative optimization algorithm.
+
.. [BT98] Birchfield, S. and Tomasi, C. A pixel dissimilarity measure that is insensitive to image sampling. IEEE Transactions on Pattern Analysis and Machine Intelligence. 1998.
diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
index 2675ad402a..816ed76f70 100644
--- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
@@ -802,7 +802,7 @@ public:
static void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);
- //! undistorts image, optinally chanes resolution and camera matrix. If Knew zero identity matrix is used
+ //! undistorts image, optionally changes resolution and camera matrix. If Knew zero identity matrix is used
static void undistortImage(InputArray distorted, OutputArray undistorted,
InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size());
From e6aa8ce93264401b2c0ad68ced33bf024849435d Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Tue, 29 Apr 2014 10:24:39 +0400
Subject: [PATCH 009/189] Corrected notes
---
...mera_calibration_and_3d_reconstruction.rst | 6 --
.../include/opencv2/calib3d/calib3d.hpp | 68 ------------------
modules/calib3d/src/fisheye.cpp | 7 +-
modules/calib3d/src/fisheye.hpp | 48 +++++++++++++
modules/calib3d/test/test_fisheye.cpp | 69 ++++++-------------
5 files changed, 74 insertions(+), 124 deletions(-)
create mode 100644 modules/calib3d/src/fisheye.hpp
diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
index 3d19b72895..710672cf52 100644
--- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
+++ b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
@@ -1702,12 +1702,6 @@ Estimates new camera matrix for undistortion or rectification.
:param P: New camera matrix (3x3) or new projection matrix (3x4)
- :param new_size: New size
-
- :param balance: Balance.
-
- :param fov_scale: Field of View scale.
-
Fisheye::stereoRectify
------------------------------
Stereo rectification for fisheye camera model
diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
index 816ed76f70..8caebd363c 100644
--- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
@@ -745,32 +745,10 @@ CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst,
OutputArray out, OutputArray inliers,
double ransacThreshold=3, double confidence=0.99);
-
class Fisheye
{
public:
- //Definitions:
- // Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
- // The coordinate vector of P in the camera reference frame is: Xc = R*X + T
- // where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
- // call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
- // The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
- // call r^2 = a^2 + b^2,
- // call theta = atan(r),
- //
- // Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8)
- //
- // The distorted point coordinates are: xd = [xx;yy] where:
- //
- // xx = (theta_d / r) * x
- // yy = (theta_d / r) * y
- //
- // Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
- //
- // xxp = f(1)*(xx + alpha*yy) + c(1)
- // yyp = f(2)*yy + c(2)
-
enum{
CALIB_USE_INTRINSIC_GUESS = 1,
CALIB_RECOMPUTE_EXTRINSIC = 2,
@@ -826,53 +804,7 @@ public:
double balance = 0.0, double fov_scale = 1.0);
};
-
-
-namespace internal {
-
-struct IntrinsicParams
-{
- Vec2d f;
- Vec2d c;
- Vec4d k;
- double alpha;
- std::vector isEstimate;
-
- IntrinsicParams();
- IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
- IntrinsicParams operator+(const Mat& a);
- IntrinsicParams& operator =(const Mat& a);
- void Init(const cv::Vec2d& f, const cv::Vec2d& c, const cv::Vec4d& k = Vec4d(0,0,0,0), const double& alpha = 0);
-};
-
-void projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
- cv::InputArray _rvec,cv::InputArray _tvec,
- const IntrinsicParams& param, cv::OutputArray jacobian);
-
-void ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
- Mat& tvec, Mat& J, const int MaxIter,
- const IntrinsicParams& param, const double thresh_cond);
-Mat ComputeHomography(Mat m, Mat M);
-
-Mat NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param);
-
-void InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk);
-
-void CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
- const IntrinsicParams& param, const int check_cond,
- const double thresh_cond, InputOutputArray omc, InputOutputArray Tc);
-
-void ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
- const IntrinsicParams& param, InputArray omc, InputArray Tc,
- const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3);
-
-void EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
- const IntrinsicParams& params, InputArray omc, InputArray Tc,
- IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms);
-
-}
}
#endif
-
#endif
diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp
index 0dcc5e70fd..a9172c9b26 100644
--- a/modules/calib3d/src/fisheye.cpp
+++ b/modules/calib3d/src/fisheye.cpp
@@ -1,9 +1,7 @@
#include "opencv2/opencv.hpp"
#include "opencv2/core/affine.hpp"
#include "opencv2/core/affine.hpp"
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// cv::Fisheye::projectPoints
+#include "fisheye.hpp"
namespace cv { namespace
{
@@ -16,6 +14,9 @@ namespace cv { namespace
};
}}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::projectPoints
+
void cv::Fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
InputArray K, InputArray D, double alpha, OutputArray jacobian)
{
diff --git a/modules/calib3d/src/fisheye.hpp b/modules/calib3d/src/fisheye.hpp
new file mode 100644
index 0000000000..e000e635a0
--- /dev/null
+++ b/modules/calib3d/src/fisheye.hpp
@@ -0,0 +1,48 @@
+#ifndef FISHEYE_INTERNAL_H
+#define FISHEYE_INTERNAL_H
+
+namespace cv { namespace internal {
+
+struct IntrinsicParams
+{
+ Vec2d f;
+ Vec2d c;
+ Vec4d k;
+ double alpha;
+ std::vector isEstimate;
+
+ IntrinsicParams();
+ IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
+ IntrinsicParams operator+(const Mat& a);
+ IntrinsicParams& operator =(const Mat& a);
+ void Init(const cv::Vec2d& f, const cv::Vec2d& c, const cv::Vec4d& k = Vec4d(0,0,0,0), const double& alpha = 0);
+};
+
+void projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
+ cv::InputArray _rvec,cv::InputArray _tvec,
+ const IntrinsicParams& param, cv::OutputArray jacobian);
+
+void ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
+ Mat& tvec, Mat& J, const int MaxIter,
+ const IntrinsicParams& param, const double thresh_cond);
+Mat ComputeHomography(Mat m, Mat M);
+
+Mat NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param);
+
+void InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk);
+
+void CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, const int check_cond,
+ const double thresh_cond, InputOutputArray omc, InputOutputArray Tc);
+
+void ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& param, InputArray omc, InputArray Tc,
+ const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3);
+
+void EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
+ const IntrinsicParams& params, InputArray omc, InputArray Tc,
+ IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms);
+
+}}
+
+#endif
diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp
index e7d4b311c7..41bbfea59d 100644
--- a/modules/calib3d/test/test_fisheye.cpp
+++ b/modules/calib3d/test/test_fisheye.cpp
@@ -1,6 +1,7 @@
#include "test_precomp.hpp"
-#include
+#include
#include
+#include "../src/fisheye.hpp"
class FisheyeTest : public ::testing::Test {
@@ -21,9 +22,9 @@ protected:
std::string combine_format(const std::string& item1, const std::string& item2, ...);
- void readPoins(std::vector >& objectPoints,
+ void readPoints(std::vector >& objectPoints,
std::vector >& imagePoints,
- const std::string& path, const int n_images, const int n_points);
+ const std::string& path, const int n_images);
void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q);
@@ -104,8 +105,6 @@ TEST_F(FisheyeTest, undistortImage)
else
EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
}
-
- cv::waitKey();
}
TEST_F(FisheyeTest, jacobians)
@@ -206,13 +205,11 @@ TEST_F(FisheyeTest, jacobians)
TEST_F(FisheyeTest, Calibration)
{
const int n_images = 34;
- const int n_points = 48;
- cv::Size imageSize = cv::Size(1280, 800);
std::vector > imagePoints;
std::vector > objectPoints;
- readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+ readPoints(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images);
int flag = 0;
flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
@@ -232,13 +229,11 @@ TEST_F(FisheyeTest, Calibration)
TEST_F(FisheyeTest, Homography)
{
const int n_images = 1;
- const int n_points = 48;
- cv::Size imageSize = cv::Size(1280, 800);
std::vector > imagePoints;
std::vector > objectPoints;
- readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+ readPoints(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images);
cv::internal::IntrinsicParams param;
param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
@@ -283,13 +278,11 @@ TEST_F(FisheyeTest, Homography)
TEST_F(FisheyeTest, EtimateUncertainties)
{
const int n_images = 34;
- const int n_points = 48;
- cv::Size imageSize = cv::Size(1280, 800);
std::vector > imagePoints;
std::vector > objectPoints;
- readPoins(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images, n_points);
+ readPoints(objectPoints, imagePoints, combine(datasets_repository_path, "calib-3_stereo_from_JY/left"), n_images);
int flag = 0;
flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
@@ -325,7 +318,7 @@ TEST_F(FisheyeTest, EtimateUncertainties)
EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
CV_Assert(abs(rms - 0.263782587133546) < 1e-10);
CV_Assert(errors.alpha == 0);
- }
+}
TEST_F(FisheyeTest, rectify)
{
@@ -375,7 +368,6 @@ TEST_F(FisheyeTest, rectify)
}
}
-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// FisheyeTest::
@@ -393,7 +385,6 @@ const cv::Matx33d FisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-0
const cv::Vec3d FisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
-
std::string FisheyeTest::combine(const std::string& _item1, const std::string& _item2)
{
std::string item1 = _item1, item2 = _item2;
@@ -421,44 +412,28 @@ std::string FisheyeTest::combine_format(const std::string& item1, const std::str
return std::string(buffer);
}
-void FisheyeTest::readPoins(std::vector >& objectPoints,
+void FisheyeTest::readPoints(std::vector >& objectPoints,
std::vector >& imagePoints,
- const std::string& path, const int n_images, const int n_points)
+ const std::string& path, const int n_images)
{
objectPoints.resize(n_images);
imagePoints.resize(n_images);
- std::vector image(n_points);
- std::vector object(n_points);
-
- std::ifstream ipStream;
- std::ifstream opStream;
-
- for (int image_idx = 0; image_idx < n_images; image_idx++)
+ cv::FileStorage fs1(combine(path, "objectPoints.xml"), cv::FileStorage::READ);
+ CV_Assert(fs1.isOpened());
+ for (size_t i = 0; i < objectPoints.size(); ++i)
{
- std::stringstream ss;
- ss << image_idx;
- std::string idxStr = ss.str();
-
- ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
- opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in);
- CV_Assert(ipStream.is_open() && opStream.is_open());
-
- for (int point_idx = 0; point_idx < n_points; point_idx++)
- {
- double x, y, z;
- char delim;
- ipStream >> x >> delim >> y;
- image[point_idx] = cv::Point2d(x, y);
- opStream >> x >> delim >> y >> delim >> z;
- object[point_idx] = cv::Point3d(x, y, z);
- }
- ipStream.close();
- opStream.close();
+ fs1[cv::format("image_%d", i)] >> objectPoints[i];
+ }
+ fs1.release();
- imagePoints[image_idx] = image;
- objectPoints[image_idx] = object;
+ cv::FileStorage fs2(combine(path, "imagePoints.xml"), cv::FileStorage::READ);
+ CV_Assert(fs2.isOpened());
+ for (size_t i = 0; i < imagePoints.size(); ++i)
+ {
+ fs2[cv::format("image_%d", i)] >> imagePoints[i];
}
+ fs2.release();
}
void FisheyeTest::readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2,
From c2341fd4464265374ff86829f3e1c8aad0254b50 Mon Sep 17 00:00:00 2001
From: Ilya Krylov
Date: Mon, 5 May 2014 14:21:24 +0400
Subject: [PATCH 010/189] Added stereoCalibrate for Fisheye camera model
---
.../include/opencv2/calib3d/calib3d.hpp | 10 +-
modules/calib3d/src/fisheye.cpp | 413 ++++++++++++++++++
modules/calib3d/src/fisheye.hpp | 12 +
3 files changed, 434 insertions(+), 1 deletion(-)
diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
index 8caebd363c..b495fbe2d0 100644
--- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp
@@ -757,7 +757,8 @@ public:
CALIB_FIX_K1 = 16,
CALIB_FIX_K2 = 32,
CALIB_FIX_K3 = 64,
- CALIB_FIX_K4 = 128
+ CALIB_FIX_K4 = 128,
+ CALIB_FIX_INTRINSIC = 256
};
//! projects 3D points using fisheye model
@@ -802,6 +803,13 @@ public:
static void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
double balance = 0.0, double fov_scale = 1.0);
+
+ //! performs stereo calibaration
+ static double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
+ InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
+ InputOutputArrayOfArrays rvecs1, InputOutputArrayOfArrays tvecs1,
+ InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
+ TermCriteria criteria = TermCriteria(3, 100, 1e-10));
};
}
diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp
index a9172c9b26..19b30428e9 100644
--- a/modules/calib3d/src/fisheye.cpp
+++ b/modules/calib3d/src/fisheye.cpp
@@ -2,6 +2,7 @@
#include "opencv2/core/affine.hpp"
#include "opencv2/core/affine.hpp"
#include "fisheye.hpp"
+#include "iomanip"
namespace cv { namespace
{
@@ -12,6 +13,8 @@ namespace cv { namespace
Vec3d dom, dT;
double dalpha;
};
+
+ void subMatrix(const Mat& src, Mat& dst, const vector& cols, const vector& rows);
}}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -757,6 +760,297 @@ double cv::Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
return rms;
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// cv::Fisheye::stereoCalibrate
+
+double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
+ InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
+ InputOutputArray rvecs1, InputOutputArrayOfArrays tvecs1,
+ InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
+ TermCriteria criteria)
+{
+ CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty());
+ CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total());
+ CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
+ CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2);
+ CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2);
+
+ CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty());
+ CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty());
+ CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty());
+ CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty());
+
+ CV_Assert((!rvecs1.empty() && rvecs1.channels() == 3) || rvecs1.empty());
+ CV_Assert((!tvecs1.empty() && tvecs1.channels() == 3) || tvecs1.empty());
+ CV_Assert((!rvecs2.empty() && rvecs2.channels() == 3) || rvecs2.empty());
+ CV_Assert((!tvecs2.empty() && tvecs2.channels() == 3) || tvecs2.empty());
+
+ //-------------------------------Initialization
+
+ const int threshold = 50;
+
+ size_t n_points = objectPoints.getMat(0).total();
+ size_t n_images = objectPoints.total();
+
+ double change = 1;
+
+ cv::internal::IntrinsicParams intrinsicLeft;
+ cv::internal::IntrinsicParams intrinsicRight;
+
+ cv::internal::IntrinsicParams intrinsicLeft_errors;
+ cv::internal::IntrinsicParams intrinsicRight_errors;
+
+ Matx33d _K;
+ Vec4d _D;
+
+ K1.getMat().convertTo(_K, CV_64FC1);
+ D1.getMat().convertTo(_D, CV_64FC1);
+ intrinsicLeft.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
+ Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
+
+ K2.getMat().convertTo(_K, CV_64FC1);
+ D2.getMat().convertTo(_D, CV_64FC1);
+ intrinsicRight.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
+ Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
+
+ intrinsicLeft.isEstimate[0] = 1;
+ intrinsicLeft.isEstimate[1] = 1;
+ intrinsicLeft.isEstimate[2] = 1;
+ intrinsicLeft.isEstimate[3] = 1;
+ intrinsicLeft.isEstimate[4] = 0;
+ intrinsicLeft.isEstimate[5] = 1;
+ intrinsicLeft.isEstimate[6] = 1;
+ intrinsicLeft.isEstimate[7] = 1;
+ intrinsicLeft.isEstimate[8] = 1;
+
+ intrinsicRight.isEstimate[0] = 1;
+ intrinsicRight.isEstimate[1] = 1;
+ intrinsicRight.isEstimate[2] = 1;
+ intrinsicRight.isEstimate[3] = 1;
+ intrinsicRight.isEstimate[4] = 0;
+ intrinsicRight.isEstimate[5] = 1;
+ intrinsicRight.isEstimate[6] = 1;
+ intrinsicRight.isEstimate[7] = 1;
+ intrinsicRight.isEstimate[8] = 1;
+
+ intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
+ intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;
+
+ std::vector selectedParams;
+ std::vector tmp(6 * (n_images + 1), 1);
+ selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end());
+ selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end());
+ selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end());
+
+ //Init values for rotation and translation between two views
+ cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3);
+ cv::Mat om_ref, R_ref, T_ref, R1, R2;
+ for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
+ {
+ cv::Rodrigues(rvecs1.getMat(image_idx), R1);
+ cv::Rodrigues(rvecs2.getMat(image_idx), R2);
+ R_ref = R2 * R1.t();
+ T_ref = tvecs2.getMat(image_idx).reshape(1, 3) - R_ref * tvecs1.getMat(image_idx).reshape(1, 3);
+ cv::Rodrigues(R_ref, om_ref);
+ om_ref.reshape(3, 1).copyTo(om_list.col(image_idx));
+ T_ref.reshape(3, 1).copyTo(T_list.col(image_idx));
+ }
+ cv::Vec3d omcur = internal::median3d(om_list);
+ cv::Vec3d Tcur = internal::median3d(T_list);
+
+ cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1),
+ e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk;
+ cv::Mat J2_inv;
+ for(int iter = 0; ; ++iter)
+ {
+ if ((criteria.type == 1 && iter >= criteria.maxCount) ||
+ (criteria.type == 2 && change <= criteria.epsilon) ||
+ (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
+ break;
+
+ J.create(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1);
+ e.create(4 * n_points * n_images, 1, CV_64FC1);
+ Jkk.create(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
+ ekk.create(4 * n_points, 1, CV_64FC1);
+
+ cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT;
+
+ for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
+ {
+ Jkk = cv::Mat::zeros(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
+
+ cv::Mat object = objectPoints.getMat(image_idx).clone();
+ cv::Mat imageLeft = imagePoints1.getMat(image_idx).clone();
+ cv::Mat imageRight = imagePoints2.getMat(image_idx).clone();
+ cv::Mat jacobians, projected;
+
+ //left camera jacobian
+ cv::Mat rvec = rvecs1.getMat(image_idx).clone();
+ cv::Mat tvec = tvecs1.getMat(image_idx).clone();
+ cv::internal::projectPoints(object, projected, rvec, tvec, intrinsicLeft, jacobians);
+ cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(0, 2 * n_points));
+ jacobians.colRange(8, 11).copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(0, 2 * n_points));
+ jacobians.colRange(11, 14).copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(0, 2 * n_points));
+ jacobians.colRange(0, 2).copyTo(Jkk.colRange(0, 2).rowRange(0, 2 * n_points));
+ jacobians.colRange(2, 4).copyTo(Jkk.colRange(2, 4).rowRange(0, 2 * n_points));
+ jacobians.colRange(4, 8).copyTo(Jkk.colRange(5, 9).rowRange(0, 2 * n_points));
+ jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points));
+
+ //right camera jacobian
+ internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
+ rvec = rvecs2.getMat(image_idx).clone();
+ tvec = tvecs2.getMat(image_idx).clone();
+
+ cv::internal::projectPoints(object, projected, omr, Tr, intrinsicRight, jacobians);
+ cv::Mat(cv::Mat((imageRight - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(2 * n_points, 4 * n_points));
+ cv::Mat dxrdom = jacobians.colRange(8, 11) * domrdom + jacobians.colRange(11, 14) * dTrdom;
+ cv::Mat dxrdT = jacobians.colRange(8, 11) * domrdT + jacobians.colRange(11, 14)* dTrdT;
+ cv::Mat dxrdomckk = jacobians.colRange(8, 11) * domrdomckk + jacobians.colRange(11, 14) * dTrdomckk;
+ cv::Mat dxrdTckk = jacobians.colRange(8, 11) * domrdTckk + jacobians.colRange(11, 14) * dTrdTckk;
+
+ dxrdom.copyTo(Jkk.colRange(18, 21).rowRange(2 * n_points, 4 * n_points));
+ dxrdT.copyTo(Jkk.colRange(21, 24).rowRange(2 * n_points, 4 * n_points));
+ dxrdomckk.copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
+ dxrdTckk.copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
+ jacobians.colRange(0, 2).copyTo(Jkk.colRange(9 + 0, 9 + 2).rowRange(2 * n_points, 4 * n_points));
+ jacobians.colRange(2, 4).copyTo(Jkk.colRange(9 + 2, 9 + 4).rowRange(2 * n_points, 4 * n_points));
+ jacobians.colRange(4, 8).copyTo(Jkk.colRange(9 + 5, 9 + 9).rowRange(2 * n_points, 4 * n_points));
+ jacobians.col(14).copyTo(Jkk.col(9 + 4).rowRange(2 * n_points, 4 * n_points));
+
+ //check goodness of sterepair
+ double abs_max = 0;
+ for (size_t i = 0; i < 4 * n_points; i++)
+ {
+ if (fabs(ekk.at(i)) > abs_max)
+ {
+ abs_max = fabs(ekk.at(i));
+ }
+ }
+ if (abs_max < threshold)
+ {
+ Jkk.copyTo(J.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
+ ekk.copyTo(e.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
+ }
+ else
+ {
+ CV_Assert(!"Bad stereo pair");
+ }
+ }
+
+ cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
+
+ //update all parameters
+ cv::subMatrix(J, J, selectedParams, std::vector(J.rows, 1));
+ cv::Mat J2 = J.t() * J;
+ J2_inv = J2.inv();
+ int a = cv::countNonZero(intrinsicLeft.isEstimate);
+ int b = cv::countNonZero(intrinsicRight.isEstimate);
+ cv::Mat deltas = J2_inv * J.t() * e;
+ intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
+ intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
+ omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3));
+ Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6));
+ for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
+ {
+ rvecs1.getMat(image_idx) = rvecs1.getMat(image_idx) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6).reshape(3);
+ tvecs1.getMat(image_idx) = tvecs1.getMat(image_idx) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6).reshape(3);
+ }
+
+ cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
+ change = cv::norm(newTom - oldTom) / cv::norm(newTom);
+ }
+
+ //estimate uncertainties
+ cv::Mat sigma_x;
+ cv::meanStdDev(e, cv::noArray(), sigma_x);
+ sigma_x *= sqrt((double)e.total() / (e.total() - 1));
+ cv::sqrt(J2_inv, J2_inv);
+ cv::Mat errors = 3 * J2_inv.diag() * sigma_x;
+ int a1 = cv::countNonZero(intrinsicLeft_errors.isEstimate);
+ int b1 = cv::countNonZero(intrinsicRight_errors.isEstimate);
+ intrinsicLeft_errors = errors.rowRange(0, a1);
+ intrinsicRight_errors = errors.rowRange(a1, a1 + b1);
+ cv::Vec3d om_error = cv::Vec3d(errors.rowRange(a1 + b1, a1 + b1 + 3));
+ cv::Vec3d T_error = cv::Vec3d(errors.rowRange(a1 + b1 + 3, a1 + b1 + 6));
+
+ std::cout << std::setprecision(15) << "left f = " << intrinsicLeft.f << std::endl;
+ std::cout << std::setprecision(15) << "left c = " << intrinsicLeft.c << std::endl;
+ std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft.alpha << std::endl;
+ std::cout << std::setprecision(15) << "left k = " << intrinsicLeft.k << std::endl;
+
+ std::cout << std::setprecision(15) << "right f = " << intrinsicRight.f << std::endl;
+ std::cout << std::setprecision(15) << "right c = " << intrinsicRight.c << std::endl;
+ std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight.alpha << std::endl;
+ std::cout << std::setprecision(15) << "right k = " << intrinsicRight.k << std::endl;
+
+ std::cout << omcur << std::endl;
+ std::cout << Tcur << std::endl;
+ std::cout << "====================================================================================" << std::endl;
+ std::cout.flush();
+
+ std::cout << std::setprecision(15) << "left f = " << intrinsicLeft_errors.f << std::endl;
+ std::cout << std::setprecision(15) << "left c = " << intrinsicLeft_errors.c << std::endl;
+ std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft_errors.alpha << std::endl;
+ std::cout << std::setprecision(15) << "left k = " << intrinsicLeft_errors.k << std::endl;
+
+ std::cout << std::setprecision(15) << "right f = " << intrinsicRight_errors.f << std::endl;
+ std::cout << std::setprecision(15) << "right c = " << intrinsicRight_errors.c << std::endl;
+ std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight_errors.alpha << std::endl;
+ std::cout << std::setprecision(15) << "right k = " << intrinsicRight_errors.k << std::endl;
+
+ std::cout << om_error << std::endl;
+ std::cout << T_error << std::endl;
+ std::cout << "====================================================================================" << std::endl;
+ std::cout.flush();
+
+ CV_Assert(cv::norm(intrinsicLeft.f - cv::Vec2d(561.195925927249, 562.849402029712)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicLeft.c - cv::Vec2d(621.282400272412, 380.555455380889)) < 1e-12);
+ CV_Assert(intrinsicLeft.alpha == 0);
+ CV_Assert(cv::norm(intrinsicLeft.k - cv::Vec4d(-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicRight.f - cv::Vec2d(560.395452535348, 561.90171021422)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicRight.c - cv::Vec2d(678.971652040359, 380.401340535339)) < 1e-12);
+ CV_Assert(intrinsicRight.alpha == 0);
+ CV_Assert(cv::norm(intrinsicRight.k - cv::Vec4d(-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222)) < 1e-12);
+ CV_Assert(cv::norm(omcur - cv::Vec3d(-0.00605728469659871, 0.006287139337868821, -0.06960627514977027)) < 1e-12);
+ CV_Assert(cv::norm(Tcur - cv::Vec3d(-0.09940272472412097, 0.002708121392654134, 0.001293302924726987)) < 1e-12);
+
+ CV_Assert(cv::norm(intrinsicLeft_errors.f - cv::Vec2d(0.71024293066476, 0.717596249442966)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicLeft_errors.c - cv::Vec2d(0.782164491020839, 0.538718002947604)) < 1e-12);
+ CV_Assert(intrinsicLeft_errors.alpha == 0);
+ CV_Assert(cv::norm(intrinsicLeft_errors.k - cv::Vec4d(0.00525819017878291, 0.0179451746982225, 0.0236417266063274, 0.0104757238170252)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicRight_errors.f - cv::Vec2d(0.748707568264116, 0.745355483082726)) < 1e-12);
+ CV_Assert(cv::norm(intrinsicRight_errors.c - cv::Vec2d(0.788236834082615, 0.538854504490304)) < 1e-12);
+ CV_Assert(intrinsicRight_errors.alpha == 0);
+ CV_Assert(cv::norm(intrinsicRight_errors.k - cv::Vec4d(0.00534743998208779, 0.0175804116710864, 0.0226549382734192, 0.00979255348533809)) < 1e-12);
+ CV_Assert(cv::norm(om_error - cv::Vec3d(0.0005903298904975326, 0.001048251127879415, 0.0001775640833531587)) < 1e-12);
+ CV_Assert(cv::norm(T_error - cv::Vec3d(6.691282702437657e-05, 5.566841336891827e-05, 0.0001954901454589594)) < 1e-12);
+
+
+ Matx33d _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
+ 0, intrinsicLeft.f[1], intrinsicLeft.c[1],
+ 0, 0, 1);
+
+ Matx33d _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
+ 0, intrinsicRight.f[1], intrinsicRight.c[1],
+ 0, 0, 1);
+
+ Mat _R;
+ Rodrigues(omcur, _R);
+
+// if (K1.needed()) cv::Mat(_K1).convertTo(K2, K1.empty() ? CV_64FC1 : K1.type());
+// if (K2.needed()) cv::Mat(_K2).convertTo(K2, K2.empty() ? CV_64FC1 : K2.type());
+// if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(D1, D1.empty() ? CV_64FC1 : D1.type());
+// if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type());
+// if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type());
+// if (T.needed()) Tcur.convertTo(R, R.empty() ? CV_64FC1 : R.type());
+
+// if (rvecs1.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
+// if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
+
+
+ return 0;
+}
+
namespace cv{ namespace {
void subMatrix(const Mat& src, Mat& dst, const vector& cols, const vector& rows)
{
@@ -1216,3 +1510,122 @@ void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputA
rms /= ex.total();
rms = sqrt(rms);
}
+
+void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB)
+{
+ CV_Assert(A.getMat().cols == B.getMat().rows);
+ CV_Assert(A.type() == CV_64FC1 && B.type() == CV_64FC1);
+
+ size_t p = A.getMat().rows;
+ size_t n = A.getMat().cols;
+ size_t q = B.getMat().cols;
+
+ dABdA.create(p * q, p * n, CV_64FC1);
+ dABdB.create(p * q, q * n, CV_64FC1);
+
+ dABdA.getMat() = Mat::zeros(p * q, p * n, CV_64FC1);
+ dABdB.getMat() = Mat::zeros(p * q, q * n, CV_64FC1);
+
+ for (size_t i = 0; i < q; ++i)
+ {
+ for (size_t j = 0; j < p; ++j)
+ {
+ size_t ij = j + i * p;
+ for (size_t k = 0; k < n; ++k)
+ {
+ size_t kj = j + k * p;
+ dABdA.getMat().at