diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 8e3e20ece15eb99a0ea8a88e8e5b126c294b74e5..650121b46d60f3b5f2cffdce56fa83c12ac1529d 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -1304,12 +1304,12 @@ cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicPar 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); + const Vec2d* ptr = imagePoints.ptr(); + Vec2d* ptr_d = distorted.ptr(); 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]; + ptr_d[i][0] -= param.alpha * ptr_d[i][1]; } cv::fisheye::undistortPoints(distorted, undistorted, Matx33d::eye(), param.k); return undistorted; @@ -1317,12 +1317,11 @@ cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicPar 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 imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t(); + Mat objectPoints = _objectPoints.reshape(1).t(); Mat objectPointsMean, covObjectPoints; Mat Rckk; int Np = imagePointsNormalized.cols; @@ -1375,9 +1374,12 @@ void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArr objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); - InitExtrinsics(image, object, param, omckk, Tckk); + bool imT = image.rows < image.cols; + bool obT = object.rows < object.cols; + + InitExtrinsics(imT ? image.t() : image, obT ? object.t() : object, param, omckk, Tckk); - ComputeExtrinsicRefine(image, object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond); + ComputeExtrinsicRefine(!imT ? image.t() : image, !obT ? object.t() : object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond); if (check_cond) { SVD svd(JJ_kk, SVD::NO_UV); @@ -1388,7 +1390,6 @@ void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArr } } - 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, Mat& ex3) @@ -1410,12 +1411,13 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); + bool imT = image.rows < image.cols; 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 exkk = (imT ? image.t() : image) - Mat(x); Mat A(jacobians.rows, 9, CV_64FC1); jacobians.colRange(0, 4).copyTo(A.colRange(0, 4)); @@ -1469,11 +1471,13 @@ void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputA objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); + bool imT = image.rows < image.cols; + 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); + Mat ex_ = (imT ? image.t() : image) - Mat(x); ex_.copyTo(ex.rowRange(ex_.rows * image_idx, ex_.rows * (image_idx + 1))); }