32 case aruco::Dict::Original:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_ARUCO_ORIGINAL);
33 case aruco::Dict::D4X4_50:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
34 case aruco::Dict::D4X4_100:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100);
35 case aruco::Dict::D4X4_250:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
36 case aruco::Dict::D4X4_1000:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
37 case aruco::Dict::D5X5_50:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_50);
38 case aruco::Dict::D5X5_100:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_100);
39 case aruco::Dict::D5X5_250:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_250);
40 case aruco::Dict::D5X5_1000:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
41 case aruco::Dict::D6X6_50:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
42 case aruco::Dict::D6X6_100:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_100);
43 case aruco::Dict::D6X6_250:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
44 case aruco::Dict::D6X6_1000:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000);
45 case aruco::Dict::D7X7_50:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_50);
46 case aruco::Dict::D7X7_100:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_100);
47 case aruco::Dict::D7X7_250:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_250);
48 case aruco::Dict::D7X7_1000:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_1000);
49 case aruco::Dict::ATAG_16h5:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
50 case aruco::Dict::ATAG_25h9:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_25h9);
51 case aruco::Dict::ATAG_36h10:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_36h10);
52 case aruco::Dict::ATAG_36h11:
return cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_36h11);
64 itsCalib = engine()->loadCameraCalibration();
67 aruco::dictionary::freeze(
true);
68 aruco::detparams::freeze(
true);
69 cv::aruco::DetectorParameters dparams;
71 switch (aruco::dictionary::get())
73 case aruco::Dict::ATAG_16h5:
74 case aruco::Dict::ATAG_25h9:
75 case aruco::Dict::ATAG_36h10:
76 case aruco::Dict::ATAG_36h11:
77 dparams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG;
81 dparams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
85 std::string
const dpf = aruco::detparams::get();
86 if (dpf.empty() ==
false)
88 cv::FileStorage fs(dpf, cv::FileStorage::READ);
91 if (dparams.readDetectorParameters(fs.root()) ==
false)
92 LERROR(
"Error reading ArUco detector parameters from file [" << dpf <<
"] -- IGNORED");
94 else LERROR(
"Failed to read ArUco detector parameters from file [" << dpf <<
"] -- IGNORED");
98 cv::aruco::Dictionary dico =
getDictionary(aruco::dictionary::get());
101 itsDetector = cv::Ptr<cv::aruco::ArucoDetector>(
new cv::aruco::ArucoDetector(dico, dparams));
128 unsigned int w,
unsigned int h, std::vector<cv::Vec3d>
const & rvecs,
129 std::vector<cv::Vec3d>
const & tvecs)
131 if (rvecs.empty() ==
false)
133 float const siz = markerlen::get();
136 for (
size_t i = 0; i < corners.size(); ++i)
138 cv::Vec3d
const & rv = rvecs[i];
139 cv::Vec3d
const & tv = tvecs[i];
142 float theta = std::sqrt(rv[0] * rv[0] + rv[1] * rv[1] + rv[2] * rv[2]);
143 Eigen::Vector3f axis(rv[0], rv[1], rv[2]);
144 Eigen::Quaternion<float> q(Eigen::AngleAxis<float>(theta, axis));
148 q.w(), q.x(), q.y(), q.z(),
149 "U" + std::to_string(ids[i]));
155 for (
size_t i = 0; i < corners.size(); ++i)
157 std::vector<cv::Point2f>
const & currentMarker = corners[i];
165 std::vector<std::vector<cv::Point2f> > corners, std::vector<cv::Vec3d>
const & rvecs,
166 std::vector<cv::Vec3d>
const & tvecs)
169 int nMarkers = int(corners.size());
170 for (
int i = 0; i < nMarkers; ++i)
172 std::vector<cv::Point2f>
const & currentMarker = corners[i];
175 for (
int j = 0; j < 4; ++j)
177 cv::Point2f
const & p0 = currentMarker[j];
178 cv::Point2f
const & p1 = currentMarker[ (j+1) % 4 ];
188 if (ids.empty() ==
false)
190 cv::Point2f cent(0.0F, 0.0F);
for (
int p = 0; p < 4; ++p) cent += currentMarker[p] * 0.25F;
197 if (dopose::get() && ids.empty() ==
false)
199 float const length = markerlen::get() * 0.4F;
201 for (
size_t i = 0; i < ids.size(); ++i)
204 std::vector<cv::Point3f> axisPoints;
205 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, 0.0F));
206 axisPoints.push_back(cv::Point3f(length, 0.0F, 0.0F));
207 axisPoints.push_back(cv::Point3f(0.0F, length, 0.0F));
208 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, length));
210 std::vector<cv::Point2f> imagePoints;
215 int(imagePoints[1].x + 0.5F),
int(imagePoints[1].y + 0.5F),
218 int(imagePoints[2].x + 0.5F),
int(imagePoints[2].y + 0.5F),
221 int(imagePoints[3].x + 0.5F),
int(imagePoints[3].y + 0.5F),
227 float const len = markerlen::get() * 0.5F;
229 std::vector<cv::Point3f> cubePoints;
230 cubePoints.push_back(cv::Point3f(-len, -len, 0.0F));
231 cubePoints.push_back(cv::Point3f(len, -len, 0.0F));
232 cubePoints.push_back(cv::Point3f(len, len, 0.0F));
233 cubePoints.push_back(cv::Point3f(-len, len, 0.0F));
234 cubePoints.push_back(cv::Point3f(-len, -len, len * 2.0F));
235 cubePoints.push_back(cv::Point3f(len, -len, len * 2.0F));
236 cubePoints.push_back(cv::Point3f(len, len, len * 2.0F));
237 cubePoints.push_back(cv::Point3f(-len, len, len * 2.0F));
239 std::vector<cv::Point2f> cuf;
243 std::vector<cv::Point> cu;
244 for (
auto const & p : cuf) cu.push_back(cv::Point(
int(p.x + 0.5F),
int(p.y + 0.5F)));
264 if (txtx >=0 && txty >= 0)
272 std::vector<std::vector<cv::Point2f> > corners, std::vector<cv::Vec3d>
const & rvecs,
273 std::vector<cv::Vec3d>
const & tvecs)
275 ImU32
const col = ImColor(128, 255, 128, 255);
278 int nMarkers = int(corners.size());
279 for (
int i = 0; i < nMarkers; ++i)
281 std::vector<cv::Point2f>
const & currentMarker = corners[i];
284 helper.
drawPoly(currentMarker, col,
true);
287 helper.
drawCircle(currentMarker[0].x, currentMarker[0].y, 3.0F, col,
true);
290 if (ids.empty() ==
false)
292 cv::Point2f cent(0.0F, 0.0F);
for (
int p = 0; p < 4; ++p) cent += currentMarker[p] * 0.25F;
293 helper.
drawText(cent.x, cent.y - 10, (
"id=" + std::to_string(ids[i])).c_str(), col);
298 if (dopose::get() && ids.empty() ==
false)
300 float const length = markerlen::get() * 0.4F;
302 for (
size_t i = 0; i < ids.size(); ++i)
305 std::vector<cv::Point3f> axisPoints;
306 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, 0.0F));
307 axisPoints.push_back(cv::Point3f(length, 0.0F, 0.0F));
308 axisPoints.push_back(cv::Point3f(0.0F, length, 0.0F));
309 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, length));
311 std::vector<cv::Point2f> imagePoints;
315 helper.
drawLine(imagePoints[0].x, imagePoints[0].y, imagePoints[1].x, imagePoints[1].y, 0xff0000ff);
316 helper.
drawLine(imagePoints[0].x, imagePoints[0].y, imagePoints[2].x, imagePoints[2].y, 0xff00ff00);
317 helper.
drawLine(imagePoints[0].x, imagePoints[0].y, imagePoints[3].x, imagePoints[3].y, 0xffff0000);
322 float const len = markerlen::get() * 0.5F;
324 std::vector<cv::Point3f> cubePoints;
325 cubePoints.push_back(cv::Point3f(-len, -len, 0.0F));
326 cubePoints.push_back(cv::Point3f(len, -len, 0.0F));
327 cubePoints.push_back(cv::Point3f(len, len, 0.0F));
328 cubePoints.push_back(cv::Point3f(-len, len, 0.0F));
329 cubePoints.push_back(cv::Point3f(-len, -len, len * 2.0F));
330 cubePoints.push_back(cv::Point3f(len, -len, len * 2.0F));
331 cubePoints.push_back(cv::Point3f(len, len, len * 2.0F));
332 cubePoints.push_back(cv::Point3f(-len, len, len * 2.0F));
334 std::vector<cv::Point2f> cuf;
338 [&](
int a,
int b,
int c,
int d)
340 std::vector<cv::Point2f> p { cuf[a], cuf[b], cuf[c], cuf[d] };
345 drawface(0, 1, 2, 3);
346 drawface(0, 1, 5, 4);
347 drawface(1, 2, 6, 5);
348 drawface(2, 3, 7, 6);
349 drawface(3, 0, 4, 7);
350 drawface(4, 5, 6, 7);
355 helper.
itext(
"Detected " + std::to_string(ids.size()) +
" ArUco markers.");