From 83e32c4d37a8ddee41e7ecf5c9b42c6caa970a1d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 20 May 2024 09:42:17 +0200 Subject: [PATCH] Merge pull request #25511 from savuor:rv/hal_projectpoints HAL for projectPoints() added #25511 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/calib3d/src/calibration.cpp | 203 ++++++++++++++++++------ modules/calib3d/src/hal_replacement.hpp | 188 ++++++++++++++++++++++ 2 files changed, 343 insertions(+), 48 deletions(-) create mode 100644 modules/calib3d/src/hal_replacement.hpp diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 50940a80ff..c428da6bd5 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -41,6 +41,7 @@ //M*/ #include "precomp.hpp" +#include "hal_replacement.hpp" #include "opencv2/imgproc/imgproc_c.h" #include "distortion_model.hpp" #include "calib3d_c_api.h" @@ -516,7 +517,6 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) return 1; } - static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; static void cvProjectPoints2Internal( const CvMat* objectPoints, @@ -555,7 +555,10 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) CV_Error( cv::Error::StsBadArg, "One of required arguments is not a valid matrix" ); - int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type); + int odepth = CV_MAT_DEPTH(objectPoints->type); + int ochans = CV_MAT_CN(objectPoints->type); + int orows = objectPoints->rows, ocols = objectPoints->cols; + int total = orows * ocols * ochans; if(total % 3 != 0) { //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data @@ -563,39 +566,22 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, } count = total / 3; - if( CV_IS_CONT_MAT(objectPoints->type) && - (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&& - ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) || - (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) || - (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count))) - { - matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) )); - cvConvert(objectPoints, matM); - } - else - { -// matM = cvCreateMat( 1, count, CV_64FC3 ); -// cvConvertPointsHomogeneous( objectPoints, matM ); - CV_Error( cv::Error::StsBadArg, "Homogeneous coordinates are not supported" ); - } + CV_Assert(CV_IS_CONT_MAT(objectPoints->type)); + CV_Assert(odepth == CV_32F || odepth == CV_64F); + // Homogeneous coordinates are not supported + CV_Assert((orows == 1 && ochans == 3) || + (orows == count && ochans*ocols == 3) || + (orows == 3 && ochans == 1 && ocols == count)); - if( CV_IS_CONT_MAT(imagePoints->type) && - (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) && - ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) || - (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) || - (imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count))) - { - _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) )); - cvConvert(imagePoints, _m); - } - else - { -// _m = cvCreateMat( 1, count, CV_64FC2 ); - CV_Error( cv::Error::StsBadArg, "Homogeneous coordinates are not supported" ); - } - - M = (CvPoint3D64f*)matM->data.db; - m = (CvPoint2D64f*)_m->data.db; + int idepth = CV_MAT_DEPTH(imagePoints->type); + int ichans = CV_MAT_CN(imagePoints->type); + int irows = imagePoints->rows, icols = imagePoints->cols; + CV_Assert(CV_IS_CONT_MAT(imagePoints->type)); + CV_Assert(idepth == CV_32F || idepth == CV_64F); + // Homogeneous coordinates are not supported + CV_Assert((irows == 1 && ichans == 2) || + (irows == count && ichans*icols == 2) || + (irows == 2 && ichans == 1 && icols == count)); if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || (((r_vec->rows != 1 && r_vec->cols != 1) || @@ -638,29 +624,150 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints, if( fixedAspectRatio ) fx = fy*aspectRatio; + int delems = 0; if( distCoeffs ) { - if( !CV_IS_MAT(distCoeffs) || - (CV_MAT_DEPTH(distCoeffs->type) != CV_64F && - CV_MAT_DEPTH(distCoeffs->type) != CV_32F) || - (distCoeffs->rows != 1 && distCoeffs->cols != 1) || - (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) ) - CV_Error( cv::Error::StsBadArg, cvDistCoeffErr ); + CV_Assert(CV_IS_MAT(distCoeffs)); - _k = cvMat( distCoeffs->rows, distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k ); + int ddepth = CV_MAT_DEPTH(distCoeffs->type); + int dchans = CV_MAT_CN(distCoeffs->type); + int drows = distCoeffs->rows, dcols = distCoeffs->cols; + delems = drows * dcols * dchans; + CV_Assert((ddepth == CV_32F || ddepth == CV_64F) && + (drows == 1 || dcols == 1) && + (delems == 4 || delems == 5 || delems == 8 || delems == 12 || delems == 14)); + + _k = cvMat( drows, dcols, CV_MAKETYPE(CV_64F, dchans), k ); cvConvert( distCoeffs, &_k ); if(k[12] != 0 || k[13] != 0) { - detail::computeTiltProjectionMatrix(k[12], k[13], - &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); + detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt, &dMatTiltdTauX, &dMatTiltdTauY); } } + if (idepth == CV_32F && odepth == CV_32F) + { + float rtMatrix[12] = { (float)R[0], (float)R[1], (float)R[2], (float)t[0], + (float)R[3], (float)R[4], (float)R[5], (float)t[1], + (float)R[6], (float)R[7], (float)R[8], (float)t[2] }; + + cv_camera_intrinsics_pinhole_32f intr; + intr.fx = (float)fx; intr.fy = (float)fy; + intr.cx = (float)cx; intr.cy = (float)cy; + intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; + + switch (delems) + { + case 0: break; + case 4: // [k_1, k_2, p_1, p_2] + intr.amt_k = 2; intr.amt_p = 2; + break; + case 5: // [k_1, k_2, p_1, p_2, k_3] + intr.amt_k = 3; intr.amt_p = 2; + break; + case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] + intr.amt_k = 6; intr.amt_p = 2; + break; + case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; + break; + case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; + break; + default: + CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); + } + + intr.k[0] = (float)k[0]; + intr.k[1] = (float)k[1]; + intr.k[2] = (float)k[4]; + intr.k[3] = (float)k[5]; + intr.k[4] = (float)k[6]; + intr.k[5] = (float)k[7]; + + intr.p[0] = (float)k[2]; + intr.p[1] = (float)k[3]; + + for (int ctr = 0; ctr < 4; ctr++) + { + intr.s[ctr] = (float)k[8+ctr]; + } + + intr.tau_x = (float)k[12]; + intr.tau_y = (float)k[13]; + + CALL_HAL(projectPoints, cv_hal_project_points_pinhole32f, + objectPoints->data.fl, objectPoints->step, count, + imagePoints->data.fl, imagePoints->step, + rtMatrix, &intr); + } + + _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) )); + cvConvert(imagePoints, _m); + + matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) )); + cvConvert(objectPoints, matM); + + M = (CvPoint3D64f*)matM->data.db; + m = (CvPoint2D64f*)_m->data.db; + + if (idepth == CV_64F && odepth == CV_64F) + { + double rtMatrix[12] = { R[0], R[1], R[2], t[0], + R[3], R[4], R[5], t[1], + R[6], R[7], R[8], t[2] }; + + cv_camera_intrinsics_pinhole_64f intr; + intr.fx = fx; intr.fy = fy; + intr.cx = cx; intr.cy = cy; + intr.amt_k = 0; intr.amt_p = 0; intr.amt_s = 0; intr.use_tau = false; + + switch (delems) + { + case 0: break; + case 4: // [k_1, k_2, p_1, p_2] + intr.amt_k = 2; intr.amt_p = 2; + break; + case 5: // [k_1, k_2, p_1, p_2, k_3] + intr.amt_k = 3; intr.amt_p = 2; + break; + case 8: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6] + intr.amt_k = 6; intr.amt_p = 2; + break; + case 12: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; + break; + case 14: // [k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, tau_x, tau_y] + intr.amt_k = 6; intr.amt_p = 2; intr.amt_s = 4; intr.use_tau = true; + break; + default: + CV_Error(cv::Error::StsInternal, "Wrong number of distortion coefficients"); + } + + intr.k[0] = k[0]; + intr.k[1] = k[1]; + intr.k[2] = k[4]; + intr.k[3] = k[5]; + intr.k[4] = k[6]; + intr.k[5] = k[7]; + + intr.p[0] = k[2]; + intr.p[1] = k[3]; + + for (int ctr = 0; ctr < 4; ctr++) + { + intr.s[ctr] = k[8+ctr]; + } + + intr.tau_x = k[12]; + intr.tau_y = k[13]; + + CALL_HAL(projectPoints, cv_hal_project_points_pinhole64f, + objectPoints->data.db, objectPoints->step, count, + imagePoints->data.db, imagePoints->step, + rtMatrix, &intr); + } + if( dpdr ) { if( !CV_IS_MAT(dpdr) || diff --git a/modules/calib3d/src/hal_replacement.hpp b/modules/calib3d/src/hal_replacement.hpp new file mode 100644 index 0000000000..8874c9127b --- /dev/null +++ b/modules/calib3d/src/hal_replacement.hpp @@ -0,0 +1,188 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// 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) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. +// Copyright (C) 2015, Itseez Inc., 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*/ + +#ifndef OPENCV_CALIB3D_HAL_REPLACEMENT_HPP +#define OPENCV_CALIB3D_HAL_REPLACEMENT_HPP + +#include "opencv2/core/hal/interface.h" + +#if defined(__clang__) // clang or MSVC clang +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-parameter" +#elif defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4100) +#elif defined(__GNUC__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +//! @addtogroup calib3d_hal_interface +//! @note Define your functions to override default implementations: +//! @code +//! #undef hal_add8u +//! #define hal_add8u my_add8u +//! @endcode +//! @{ + +/** + * @brief Camera intrinsics structure, see projectPoints() documentation for details + */ +struct cv_camera_intrinsics_pinhole_32f +{ + // focal length, principal point + float fx, fy, cx, cy; + // radial distortion coefficients + float k[6]; + // amount of radial distortion coefficients passed + int amt_k; + // tangential distortion coefficients + float p[2]; + // amount of tangential distortion coefficients passed + int amt_p; + // prism distortion coefficients + float s[4]; + // amount of prism distortion coefficients passed + int amt_s; + // tilt distortion coefficients + float tau_x, tau_y; + // to use tilt distortion coefficients or not + bool use_tau; +}; + +/** + @brief Project points from 3D world space to 2D screen space using rotation and translation matrix and camera intrinsic parameters + @param src_data Pointer to 3D points array with coordinates interleaved as X, Y, Z, X, Y, Z,.. + @param src_step Step between consecutive 3D points + @param src_size Amount of points + @param dst_data Pointer to resulting projected 2D points with coordinates interleaved as u, v, u, v,.. + @param dst_step Step between consecutive projected 2D points + @param rt_data Pointer to 3x4 array containing rotation-then-translation matrix + @param intr_data Pointer to camera intrinsics structure +*/ +inline int hal_ni_project_points_pinhole32f(const float* src_data, size_t src_step, size_t src_size, + float* dst_data, size_t dst_step, const float* rt_data, + const cv_camera_intrinsics_pinhole_32f* intr_data) +{ return CV_HAL_ERROR_NOT_IMPLEMENTED; } + +//! @cond IGNORED +#define cv_hal_project_points_pinhole32f hal_ni_project_points_pinhole32f +//! @endcond + +/** + * @brief Camera intrinsics structure, see projectPoints() documentation for details + */ +struct cv_camera_intrinsics_pinhole_64f +{ + // focal length, principal point + double fx, fy, cx, cy; + // radial distortion coefficients + double k[6]; + // amount of radial distortion coefficients passed + int amt_k; + // tangential distortion coefficients + double p[2]; + // amount of tangential distortion coefficients passed + int amt_p; + // prism distortion coefficients + double s[4]; + // amount of prism distortion coefficients passed + int amt_s; + // tilt distortion coefficients + double tau_x, tau_y; + // to use tilt distortion coefficients or not + bool use_tau; +}; + +/** + @brief Project points from 3D world space to 2D screen space using rotation and translation matrix and camera intrinsic parameters + @param src_data Pointer to 3D points array with coordinates interleaved as X, Y, Z, X, Y, Z,.. + @param src_step Step between consecutive 3D points + @param src_size Amount of points + @param dst_data Pointer to resulting projected 2D points with coordinates interleaved as u, v, u, v,.. + @param dst_step Step between consecutive projected 2D points + @param rt_data Pointer to 3x4 array containing rotation-then-translation matrix + @param intr_data Pointer to camera intrinsics structure +*/ +inline int hal_ni_project_points_pinhole64f(const double* src_data, size_t src_step, size_t src_size, + double* dst_data, size_t dst_step, const double* rt_data, + const cv_camera_intrinsics_pinhole_64f* intr_data) +{ return CV_HAL_ERROR_NOT_IMPLEMENTED; } + +//! @cond IGNORED +#define cv_hal_project_points_pinhole64f hal_ni_project_points_pinhole64f +//! @endcond + +//! @} + +#if defined(__clang__) +#pragma clang diagnostic pop +#elif defined(_MSC_VER) +#pragma warning(pop) +#elif defined(__GNUC__) +#pragma GCC diagnostic pop +#endif + +#include "custom_hal.hpp" + +//! @cond IGNORED +#define CALL_HAL_RET(name, fun, retval, ...) \ + int res = __CV_EXPAND(fun(__VA_ARGS__, &retval)); \ + if (res == CV_HAL_ERROR_OK) \ + return retval; \ + else if (res != CV_HAL_ERROR_NOT_IMPLEMENTED) \ + CV_Error_(cv::Error::StsInternal, \ + ("HAL implementation " CVAUX_STR(name) " ==> " CVAUX_STR(fun) " returned %d (0x%08x)", res, res)); + + +#define CALL_HAL(name, fun, ...) \ + int res = __CV_EXPAND(fun(__VA_ARGS__)); \ + if (res == CV_HAL_ERROR_OK) \ + return; \ + else if (res != CV_HAL_ERROR_NOT_IMPLEMENTED) \ + CV_Error_(cv::Error::StsInternal, \ + ("HAL implementation " CVAUX_STR(name) " ==> " CVAUX_STR(fun) " returned %d (0x%08x)", res, res)); +//! @endcond + +#endif