Number of points in undistortPoints and solvePnPRansac and input matrices format #14423
Description
There may be a bug in solvePnPRansac when less than 6 points are provided.
In the sample code below, I'm feeding solvePnPRansac with 6 points in the form of a Nx2 and Nx3 matrices. Everything goes well. This is consistent with the documentation (https://docs.opencv.org/3.3.1/d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e) and the function asserts (e.g CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);)
When I remove one or more points, the code fails with the following assert error:
unknown location(0): fatal error: in "test_template_tracker/test_und": cv::Exception: OpenCV(3.4.3) opencv-3.4.3/modules/imgproc/src/undistort.cpp:385: error: (-215:Assertion failed) CV_IS_MAT(_src) && CV_IS_MAT(_dst) && (_src->rows == 1 || _src->cols == 1) && (_dst->rows == 1 || _dst->cols == 1) && _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 && (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2) in function 'cvUndistortPointsInternal'
The problem is that when there are less than 6 point this branch of solvePnPRansac is executed https://github.com/opencv/opencv/blob/master/modules/calib3d/src/solvepnp.cpp#L312
in which solvePnP is called using directly the input parameters of solvePnPRansac, in this case Nx3 and Nx2 single channel matrices.
In turn, solvePnP calls undistortPoints() again with the same parameters received in input. This leads to cvUndistortPointsInternal() which only takes 2-channel matrices as input in the format 1xN or Nx1. Hence the assert error.
If we look at the code of solvePnPRansac for the case of >= 6 points we see that, before calling solvePnP, https://github.com/opencv/opencv/blob/master/modules/calib3d/src/solvepnp.cpp#L373 the data is reshaped into multichannel matrices with the calls to reshape(3) and reshape(2).
So, bottom line, I think there are 2 things to fix:
- For the case of the 4 or 5 points in
solvePnPRansac, the data has to be converted to multichannel matrices as done for the other case. Something like this (https://github.com/opencv/opencv/blob/master/modules/calib3d/src/solvepnp.cpp#L312):
...
if( model_points == npoints )
{
opoints = opoints.reshape(3);
ipoints = ipoints.reshape(2);
bool result = solvePnP(opoints, ipoints, cameraMatrix, distCoeffs, _rvec, _tvec, useExtrinsicGuess, ransac_kernel_method);
if(!result)
{
if( _inliers.needed() )
_inliers.release();
undistortPoints()should not be able to take single channel Nx2 matrices as input: the documentation is right about that (https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga55c716492470bfe86b0ee9bf3a1f0f7e) but in the code (https://github.com/opencv/opencv/blob/master/modules/calib3d/src/undistort.cpp#L568) we have the following check
CV_Assert( src.isContinuous() && (src.depth() == CV_32F || src.depth() == CV_64F) &&
((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));and src.cols*src.channels() == 2 is what let Nx2 single channel matrices pass. So it should be removed and replaced with src.cols == 1 && src.channels() == 2 to accunt for Nx1 2-channel matrices.
System information (version)
- OpenCV => 3.1
- Operating System / Platform => all
- Compiler => all
Steps to reproduce
Note: it's just a sample code, the values of points and calibration are irrelevant for the matter.
std::vector<int> inliers{};
cv::Mat keypoints13D = (cv::Mat_<float>(5, 3) << 12.00604, -2.8654366, 18.472504,
7.6863389, 4.9355154, 11.146358,
14.260933, 2.8320458, 12.582781,
3.4562225, 8.2668982, 11.300434,
15.316854, 3.7486348, 12.491116);
cv::Mat keypoints22D = (cv::Mat_<float>(5, 2) << 918.1734, 196.77412,
1341.7848, 946.64838,
1309.8823, 926.85284,
1153.3813, 782.78381,
1399.0817, 488.19058);
/*
// uncomment the previous 2 lines to see that solvePnPRansac is correctly working with 6 points
cv::Mat keypoints13D = (cv::Mat_<float>(6, 3) << 12.00604, -2.8654366, 18.472504,
7.6863389, 4.9355154, 11.146358,
7.6863389, 4.9355154, 11.146358,
7.6863389, 4.9355154, 11.146358,
14.260933, 2.8320458, 12.582781,
15.316854, 3.7486348, 12.491116);
cv::Mat keypoints22D = (cv::Mat_<float>(6, 2) << 918.1734, 196.77412,
1341.7848, 946.64838,
1341.7848, 946.64838,
1341.7848, 946.64838,
1309.8823, 926.85284,
1399.0817, 488.19058);*/
cv::Mat matK{cv::Mat::eye(3,3,CV_64F)};
cv::Mat distCoeff{cv::Mat::zeros(1,5,CV_64F)};
cv::Mat rvec{cv::Mat::zeros(1,3,CV_64F)};
cv::Mat Tvec{cv::Mat::zeros(1,3,CV_64F)};
const auto& pnpParams = params.pnpParams;
cv::solvePnPRansac( keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
/*
// uncomment these two lines to see that undistortPoints actually takes Nx3 matrices making cvUndistortPointsInternal() fail
cv::Mat output;
cv::undistortPoints(keypoints22D, output, matK, distCoeff);
*/