mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-01-19 06:53:50 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #25511 from savuor:rv/hal_projectpoints

Browse Source 
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
This commit is contained in:
Rostislav Vasilikhin 2024-05-20 09:42:17 +02:00 committed by GitHub
parent e05ad56f6e
commit 83e32c4d37
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 343 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
modules/calib3d/src/calibration.cpp
View File

@ -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) ||

188
modules/calib3d/src/hal_replacement.hpp Normal file
View File

@ -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