package org.opencv.test.calib3d;
+import java.util.ArrayList;
+
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
assertEquals((1 << 17), Calib3d.CALIB_USE_LU);
assertEquals((1 << 22), Calib3d.CALIB_USE_EXTRINSIC_GUESS);
}
+
+ public void testSolvePnPGeneric_regression_16040() {
+ Mat intrinsics = Mat.eye(3, 3, CvType.CV_64F);
+ intrinsics.put(0, 0, 400);
+ intrinsics.put(1, 1, 400);
+ intrinsics.put(0, 2, 640 / 2);
+ intrinsics.put(1, 2, 480 / 2);
+
+ final int minPnpPointsNum = 4;
+
+ MatOfPoint3f points3d = new MatOfPoint3f();
+ points3d.alloc(minPnpPointsNum);
+ MatOfPoint2f points2d = new MatOfPoint2f();
+ points2d.alloc(minPnpPointsNum);
+
+ for (int i = 0; i < minPnpPointsNum; i++) {
+ double x = Math.random() * 100 - 50;
+ double y = Math.random() * 100 - 50;
+ points2d.put(i, 0, x, y); //add(new Point(x, y));
+ points3d.put(i, 0, 0, y, x); // add(new Point3(0, y, x));
+ }
+
+ ArrayList<Mat> rvecs = new ArrayList<Mat>();
+ ArrayList<Mat> tvecs = new ArrayList<Mat>();
+
+ Mat rvec = new Mat();
+ Mat tvec = new Mat();
+
+ Mat reprojectionError = new Mat(2, 1, CvType.CV_64FC1);
+
+ Calib3d.solvePnPGeneric(points3d, points2d, intrinsics, new MatOfDouble(), rvecs, tvecs, false, Calib3d.SOLVEPNP_IPPE, rvec, tvec, reprojectionError);
+
+ Mat truth_rvec = new Mat(3, 1, CvType.CV_64F);
+ truth_rvec.put(0, 0, 0, Math.PI / 2, 0);
+
+ Mat truth_tvec = new Mat(3, 1, CvType.CV_64F);
+ truth_tvec.put(0, 0, -320, -240, 400);
+
+ assertMatEqual(truth_rvec, rvecs.get(0), 10 * EPS);
+ assertMatEqual(truth_tvec, tvecs.get(0), 1000 * EPS);
+ }
}
--- /dev/null
+#!/usr/bin/env python
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class solvepnp_test(NewOpenCVTests):
+
+ def test_regression_16040(self):
+ obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32)
+ img_points = np.array(
+ [[700, 400], [700, 600], [900, 600], [900, 400]], dtype=np.float32
+ )
+
+ cameraMatrix = np.array(
+ [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32
+ )
+ distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32)
+ r = np.array([], dtype=np.float32)
+ x, r, t, e = cv.solvePnPGeneric(
+ obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r
+ )
+
+ def test_regression_16040_2(self):
+ obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32)
+ img_points = np.array(
+ [[[700, 400], [700, 600], [900, 600], [900, 400]]], dtype=np.float32
+ )
+
+ cameraMatrix = np.array(
+ [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32
+ )
+ distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32)
+ r = np.array([], dtype=np.float32)
+ x, r, t, e = cv.solvePnPGeneric(
+ obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r
+ )
+
+
+if __name__ == '__main__':
+ NewOpenCVTests.bootstrap()
CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) )
&& npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
- if (opoints.cols == 3)
- opoints = opoints.reshape(3);
- if (ipoints.cols == 2)
- ipoints = ipoints.reshape(2);
+ opoints = opoints.reshape(3, npoints);
+ ipoints = ipoints.reshape(2, npoints);
if( flags != SOLVEPNP_ITERATIVE )
useExtrinsicGuess = false;
else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
{
vector<Mat> rvecs, tvecs;
- solveP3P(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags);
+ solveP3P(opoints, ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags);
vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end());
vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end());
}
"Type of reprojectionError must be CV_32FC1 or CV_64FC1!");
Mat objectPoints, imagePoints;
- if (_opoints.depth() == CV_32F)
+ if (opoints.depth() == CV_32F)
{
- _opoints.getMat().convertTo(objectPoints, CV_64F);
+ opoints.convertTo(objectPoints, CV_64F);
}
else
{
- objectPoints = _opoints.getMat();
+ objectPoints = opoints;
}
- if (_ipoints.depth() == CV_32F)
+ if (ipoints.depth() == CV_32F)
{
- _ipoints.getMat().convertTo(imagePoints, CV_64F);
+ ipoints.convertTo(imagePoints, CV_64F);
}
else
{
- imagePoints = _ipoints.getMat();
+ imagePoints = ipoints;
}
for (size_t i = 0; i < vec_rvecs.size(); i++)
{
vector<Point2d> projectedPoints;
projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints);
- double rmse = norm(projectedPoints, imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());
+ double rmse = norm(Mat(projectedPoints, false), imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());
Mat err = reprojectionError.getMat();
if (type == CV_32F)
{
- err.at<float>(0,static_cast<int>(i)) = static_cast<float>(rmse);
+ err.at<float>(static_cast<int>(i)) = static_cast<float>(rmse);
}
else
{
- err.at<double>(0,static_cast<int>(i)) = rmse;
+ err.at<double>(static_cast<int>(i)) = rmse;
}
}
}
{
CV_INSTRUMENT_REGION();
- CV_Assert( _src1.sameSize(_src2) && _src1.type() == _src2.type() );
+ CV_CheckTypeEQ(_src1.type(), _src2.type(), "Input type mismatch");
+ CV_Assert(_src1.sameSize(_src2));
#if defined HAVE_OPENCL || defined HAVE_IPP
double _result = 0;