@@ -59,64 +59,19 @@ int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr<Boar
5959return (int )objPoints.total () /4 ;
6060}
6161
62-
63- /* *
64- * Check if a set of 3d points are enough for calibration. Z coordinate is ignored.
65- * Only axis parallel lines are considered
66- */
67- static bool _arePointsEnoughForPoseEstimation (
const vector<Point3f> &points) {
68- if (points.size () <4 )return false ;
69-
70- vector<double > sameXValue;// different x values in points
71- vector<int > sameXCounter;// number of points with the x value in sameXValue
72- for (unsigned int i =0 ; i < points.size (); i++) {
73- bool found =false ;
74- for (unsigned int j =0 ; j < sameXValue.size (); j++) {
75- if (sameXValue[j] == points[i].x ) {
76- found =true ;
77- sameXCounter[j]++;
78- }
79- }
80- if (!found) {
81- sameXValue.push_back (points[i].x );
82- sameXCounter.push_back (1 );
83- }
84- }
85-
86- // count how many x values has more than 2 points
87- int moreThan2 =0 ;
88- for (unsigned int i =0 ; i < sameXCounter.size (); i++) {
89- if (sameXCounter[i] >=2 ) moreThan2++;
90- }
91-
92- // if we have more than 1 two xvalues with more than 2 points, calibration is ok
93- if (moreThan2 >1 )
94- return true ;
95- return false ;
96- }
97-
9862bool estimatePoseCharucoBoard (InputArray charucoCorners, InputArray charucoIds,
99- const Ptr<CharucoBoard> &board, InputArray cameraMatrix,
100-
101- bool useExtrinsicGuess) {
63+ const Ptr<CharucoBoard> &board, InputArray cameraMatrix,
64+ InputArray distCoeffs, InputOutputArray rvec,
65+ InputOutputArray tvec,bool useExtrinsicGuess) {
10266CV_Assert ((charucoCorners.getMat ().total () == charucoIds.getMat ().total ()));
103-
104- // need, at least, 4 corners
10567if (charucoIds.getMat ().total () <4 )return false ;
10668
107- vector<Point3f> objPoints;
108- objPoints.reserve (charucoIds.getMat ().total ());
109- for (unsigned int i =0 ; i < charucoIds.getMat ().total (); i++) {
110- int currId = charucoIds.getMat ().at <int >(i);
111- CV_Assert (currId >=0 && currId < (int )board->getChessboardCorners ().size ());
112- objPoints.push_back (board->getChessboardCorners ()[currId]);
113- }
114-
115- // points need to be in different lines, check if detected points are enough
116- if (!_arePointsEnoughForPoseEstimation (objPoints))return false ;
69+ // get object and image points for the solvePnP function
70+ Mat objPoints, imgPoints;
71+ board->matchImagePoints (charucoCorners, charucoIds, objPoints, imgPoints);
72+ solvePnP (objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess);
11773
118- solvePnP (objPoints, charucoCorners, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess);
119- return true ;
74+ return objPoints.total () >0ull ;
12075}
12176
12277bool testCharucoCornersCollinear (const Ptr<CharucoBoard> &board, InputArray charucoIds) {