|
5 | 5 | #include"precomp.hpp" |
6 | 6 | #include"opencv2/aruco.hpp" |
7 | 7 | #include<opencv2/calib3d.hpp> |
| 8 | +#include<opencv2/core/utils/logger.hpp> |
8 | 9 |
|
9 | 10 | namespacecv { |
10 | 11 | namespacearuco { |
@@ -59,64 +60,25 @@ int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr<Boar |
59 | 60 | return (int)objPoints.total() /4; |
60 | 61 | } |
61 | 62 |
|
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 | | -staticbool_arePointsEnoughForPoseEstimation( const vector<Point3f> &points) { |
68 | | -if(points.size() <4)returnfalse; |
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(unsignedint i =0; i < points.size(); i++) { |
73 | | -bool found =false; |
74 | | -for(unsignedint 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(unsignedint 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 | | -returntrue; |
95 | | -returnfalse; |
96 | | -} |
97 | | - |
98 | 63 | boolestimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, |
99 | | -const Ptr<CharucoBoard> &board, InputArray cameraMatrix, |
100 | | -InputArray distCoeffs, InputOutputArray rvec, |
101 | | -InputOutputArray tvec,bool useExtrinsicGuess) { |
| 64 | +const Ptr<CharucoBoard> &board, InputArray cameraMatrix, |
| 65 | + InputArray distCoeffs, InputOutputArray rvec, |
| 66 | + InputOutputArray tvec,bool useExtrinsicGuess) { |
102 | 67 | CV_Assert((charucoCorners.getMat().total() == charucoIds.getMat().total())); |
103 | | - |
104 | | -// need, at least, 4 corners |
105 | 68 | if(charucoIds.getMat().total() <4)returnfalse; |
106 | 69 |
|
107 | | - vector<Point3f> objPoints; |
108 | | - objPoints.reserve(charucoIds.getMat().total()); |
109 | | -for(unsignedint 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]); |
| 70 | +// get object and image points for the solvePnP function |
| 71 | + Mat objPoints, imgPoints; |
| 72 | + board->matchImagePoints(charucoCorners, charucoIds, objPoints, imgPoints); |
| 73 | +try { |
| 74 | +solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); |
| 75 | + } |
| 76 | +catch (const cv::Exception& e) { |
| 77 | +CV_LOG_WARNING(NULL,"estimatePoseCharucoBoard:" << std::endl << e.what()); |
| 78 | +returnfalse; |
113 | 79 | } |
114 | 80 |
|
115 | | -// points need to be in different lines, check if detected points are enough |
116 | | -if(!_arePointsEnoughForPoseEstimation(objPoints))returnfalse; |
117 | | - |
118 | | -solvePnP(objPoints, charucoCorners, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); |
119 | | -returntrue; |
| 81 | +return objPoints.total() >0ull; |
120 | 82 | } |
121 | 83 |
|
122 | 84 | booltestCharucoCornersCollinear(const Ptr<CharucoBoard> &board, InputArray charucoIds) { |
|