1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
#include <iostream>
#include <vector>
#include <opencv2/highgui.hpp>
#include <opencv2/objdetect/aruco_detector.hpp>
#include "aruco_samples_utility.hpp"
using namespace std;
using namespace cv;
namespace {
const char* about = "Pose estimation using a ArUco Planar Grid board";
//! [aruco_detect_board_keys]
const char* keys =
"{w | | Number of squares in X direction }"
"{h | | Number of squares in Y direction }"
"{l | | Marker side length (in pixels) }"
"{s | | Separation between two consecutive markers in the grid (in pixels)}"
"{d | | dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2,"
"DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, "
"DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12,"
"DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
"{cd | | Input file with custom dictionary }"
"{c | | Output file with calibrated camera parameters }"
"{v | | Input from video or image file, if omitted, input comes from camera }"
"{ci | 0 | Camera id if input doesnt come from video (-v) }"
"{dp | | File of marker detector parameters }"
"{rs | | Apply refind strategy }"
"{r | | show rejected candidates too }";
}
//! [aruco_detect_board_keys]
int main(int argc, char *argv[]) {
CommandLineParser parser(argc, argv, keys);
parser.about(about);
if(argc < 7) {
parser.printMessage();
return 0;
}
//! [aruco_detect_board_full_sample]
int markersX = parser.get<int>("w");
int markersY = parser.get<int>("h");
float markerLength = parser.get<float>("l");
float markerSeparation = parser.get<float>("s");
bool showRejected = parser.has("r");
bool refindStrategy = parser.has("rs");
int camId = parser.get<int>("ci");
Mat camMatrix, distCoeffs;
readCameraParamsFromCommandLine(parser, camMatrix, distCoeffs);
aruco::Dictionary dictionary = readDictionatyFromCommandLine(parser);
aruco::DetectorParameters detectorParams = readDetectorParamsFromCommandLine(parser);
String video;
if(parser.has("v")) {
video = parser.get<String>("v");
}
if(!parser.check()) {
parser.printErrors();
return 0;
}
aruco::ArucoDetector detector(dictionary, detectorParams);
VideoCapture inputVideo;
int waitTime;
if(!video.empty()) {
inputVideo.open(video);
waitTime = 0;
} else {
inputVideo.open(camId);
waitTime = 10;
}
float axisLength = 0.5f * ((float)min(markersX, markersY) * (markerLength + markerSeparation) +
markerSeparation);
// Create GridBoard object
//! [aruco_create_board]
aruco::GridBoard board(Size(markersX, markersY), markerLength, markerSeparation, dictionary);
//! [aruco_create_board]
// Also you could create Board object
//vector<vector<Point3f> > objPoints; // array of object points of all the marker corners in the board
//vector<int> ids; // vector of the identifiers of the markers in the board
//aruco::Board board(objPoints, dictionary, ids);
double totalTime = 0;
int totalIterations = 0;
while(inputVideo.grab()) {
Mat image, imageCopy;
inputVideo.retrieve(image);
double tick = (double)getTickCount();
vector<int> ids;
vector<vector<Point2f>> corners, rejected;
Vec3d rvec, tvec;
//! [aruco_detect_and_refine]
// Detect markers
detector.detectMarkers(image, corners, ids, rejected);
// Refind strategy to detect more markers
if(refindStrategy)
detector.refineDetectedMarkers(image, board, corners, ids, rejected, camMatrix,
distCoeffs);
//! [aruco_detect_and_refine]
// Estimate board pose
int markersOfBoardDetected = 0;
if(!ids.empty()) {
// Get object and image points for the solvePnP function
cv::Mat objPoints, imgPoints;
board.matchImagePoints(corners, ids, objPoints, imgPoints);
// Find pose
cv::solvePnP(objPoints, imgPoints, camMatrix, distCoeffs, rvec, tvec);
markersOfBoardDetected = (int)objPoints.total() / 4;
}
double currentTime = ((double)getTickCount() - tick) / getTickFrequency();
totalTime += currentTime;
totalIterations++;
if(totalIterations % 30 == 0) {
cout << "Detection Time = " << currentTime * 1000 << " ms "
<< "(Mean = " << 1000 * totalTime / double(totalIterations) << " ms)" << endl;
}
// Draw results
image.copyTo(imageCopy);
if(!ids.empty())
aruco::drawDetectedMarkers(imageCopy, corners, ids);
if(showRejected && !rejected.empty())
aruco::drawDetectedMarkers(imageCopy, rejected, noArray(), Scalar(100, 0, 255));
if(markersOfBoardDetected > 0)
cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength);
imshow("out", imageCopy);
char key = (char)waitKey(waitTime);
if(key == 27) break;
//! [aruco_detect_board_full_sample]
}
return 0;
}
|
