JeVoisBase  1.22
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
Loading...
Searching...
No Matches
ArUco.C
Go to the documentation of this file.
1// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2//
3// JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4// California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5//
6// This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7// redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8// Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10// License for more details. You should have received a copy of the GNU General Public License along with this program;
11// if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12//
13// Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14// Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16/*! \file */
17
20#include <jevois/Core/Module.H>
21#include <jevois/Core/Engine.H>
22#include <opencv2/calib3d.hpp> // for projectPoints()
23#include <opencv2/imgproc/imgproc.hpp>
24
25#include <Eigen/Geometry> // for AngleAxis and Quaternion
26
27// ##############################################################################################################
28cv::aruco::Dictionary ArUco::getDictionary(aruco::Dict d)
29{
30 switch (d)
31 {
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);
53 }
54}
55
56// ##############################################################################################################
59
60// ##############################################################################################################
62{
63 //! Load camera calibration:
64 itsCalib = engine()->loadCameraCalibration();
65
66 // Init detector parameters:
67 aruco::dictionary::freeze(true);
68 aruco::detparams::freeze(true);
69 cv::aruco::DetectorParameters dparams;
70
71 switch (aruco::dictionary::get())
72 {
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;
78 break;
79
80 default:
81 dparams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
82 }
83
84 // Read detector parameters if any:
85 std::string const dpf = aruco::detparams::get();
86 if (dpf.empty() == false)
87 {
88 cv::FileStorage fs(dpf, cv::FileStorage::READ);
89 if (fs.isOpened())
90 {
91 if (dparams.readDetectorParameters(fs.root()) == false)
92 LERROR("Error reading ArUco detector parameters from file [" << dpf <<"] -- IGNORED");
93 }
94 else LERROR("Failed to read ArUco detector parameters from file [" << dpf << "] -- IGNORED");
95 }
96
97 // Instantiate the dictionary:
98 cv::aruco::Dictionary dico = getDictionary(aruco::dictionary::get());
99
100 // Instantiate the detector (we use default refinement parameters):
101 itsDetector = cv::Ptr<cv::aruco::ArucoDetector>(new cv::aruco::ArucoDetector(dico, dparams));
102}
103
104// ##############################################################################################################
106{
107 itsDetector.release();
109 aruco::detparams::freeze(false);
110 aruco::dictionary::freeze(false);
111}
112
113// ##############################################################################################################
114void ArUco::detectMarkers(cv::InputArray image, cv::OutputArray ids, cv::OutputArrayOfArrays corners)
115{
116 itsDetector->detectMarkers(image, corners, ids);
117}
118
119// ##############################################################################################################
120void ArUco::estimatePoseSingleMarkers(cv::InputArrayOfArrays corners, cv::OutputArray rvecs, cv::OutputArray tvecs)
121{
122 cv::aruco::estimatePoseSingleMarkers(corners, markerlen::get(),
123 itsCalib.camMatrix, itsCalib.distCoeffs, rvecs, tvecs);
124}
125
126// ##############################################################################################################
127void ArUco::sendSerial(jevois::StdModule * mod, std::vector<int> ids, std::vector<std::vector<cv::Point2f> > corners,
128 unsigned int w, unsigned int h, std::vector<cv::Vec3d> const & rvecs,
129 std::vector<cv::Vec3d> const & tvecs)
130{
131 if (rvecs.empty() == false)
132 {
133 float const siz = markerlen::get();
134
135 // If we have rvecs and tvecs, we are doing 3D pose estimation, so send a 3D message:
136 for (size_t i = 0; i < corners.size(); ++i)
137 {
138 cv::Vec3d const & rv = rvecs[i];
139 cv::Vec3d const & tv = tvecs[i];
140
141 // Compute quaternion:
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));
145
146 mod->sendSerialStd3D(tv[0], tv[1], tv[2], // position
147 siz, siz, 1.0F, // size
148 q.w(), q.x(), q.y(), q.z(), // pose
149 "U" + std::to_string(ids[i])); // decoded ID with "U" prefix for ArUco
150 }
151 }
152 else
153 {
154 // Send one 2D message per marker:
155 for (size_t i = 0; i < corners.size(); ++i)
156 {
157 std::vector<cv::Point2f> const & currentMarker = corners[i];
158 mod->sendSerialContour2D(w, h, currentMarker, "U" + std::to_string(ids[i]));
159 }
160 }
161}
162
163// ##############################################################################################################
164void ArUco::drawDetections(jevois::RawImage & outimg, int txtx, int txty, std::vector<int> ids,
165 std::vector<std::vector<cv::Point2f> > corners, std::vector<cv::Vec3d> const & rvecs,
166 std::vector<cv::Vec3d> const & tvecs)
167{
168 // This code is like drawDetectedMarkers() in cv::aruco, but for YUYV output image:
169 int nMarkers = int(corners.size());
170 for (int i = 0; i < nMarkers; ++i)
171 {
172 std::vector<cv::Point2f> const & currentMarker = corners[i];
173
174 // draw marker sides and prepare serial out string:
175 for (int j = 0; j < 4; ++j)
176 {
177 cv::Point2f const & p0 = currentMarker[j];
178 cv::Point2f const & p1 = currentMarker[ (j+1) % 4 ];
179 jevois::rawimage::drawLine(outimg, int(p0.x + 0.5F), int(p0.y + 0.5F),
180 int(p1.x + 0.5F), int(p1.y + 0.5F), 1, jevois::yuyv::LightGreen);
181 }
182
183 // draw first corner mark
184 jevois::rawimage::drawDisk(outimg, int(currentMarker[0].x + 0.5F), int(currentMarker[0].y + 0.5F),
186
187 // draw ID
188 if (ids.empty() == false)
189 {
190 cv::Point2f cent(0.0F, 0.0F); for (int p = 0; p < 4; ++p) cent += currentMarker[p] * 0.25F;
191 jevois::rawimage::writeText(outimg, std::string("id=") + std::to_string(ids[i]),
192 int(cent.x + 0.5F), int(cent.y + 0.5F) - 5, jevois::yuyv::LightGreen);
193 }
194 }
195
196 // This code is like drawAxis() in cv::aruco, but for YUYV output image:
197 if (dopose::get() && ids.empty() == false)
198 {
199 float const length = markerlen::get() * 0.4F;
200
201 for (size_t i = 0; i < ids.size(); ++i)
202 {
203 // Project axis points:
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));
209
210 std::vector<cv::Point2f> imagePoints;
211 cv::projectPoints(axisPoints, rvecs[i], tvecs[i], itsCalib.camMatrix, itsCalib.distCoeffs, imagePoints);
212
213 // Draw axis lines
214 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
215 int(imagePoints[1].x + 0.5F), int(imagePoints[1].y + 0.5F),
217 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
218 int(imagePoints[2].x + 0.5F), int(imagePoints[2].y + 0.5F),
220 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
221 int(imagePoints[3].x + 0.5F), int(imagePoints[3].y + 0.5F),
223
224 // Also draw a cube if requested:
225 if (showcube::get())
226 {
227 float const len = markerlen::get() * 0.5F;
228
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));
238
239 std::vector<cv::Point2f> cuf;
240 cv::projectPoints(cubePoints, rvecs[i], tvecs[i], itsCalib.camMatrix, itsCalib.distCoeffs, cuf);
241
242 // Round all the coordinates:
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)));
245
246 // Draw cube lines:
247 jevois::rawimage::drawLine(outimg, cu[0].x, cu[0].y, cu[1].x, cu[1].y, 2, jevois::yuyv::LightGreen);
248 jevois::rawimage::drawLine(outimg, cu[1].x, cu[1].y, cu[2].x, cu[2].y, 2, jevois::yuyv::LightGreen);
249 jevois::rawimage::drawLine(outimg, cu[2].x, cu[2].y, cu[3].x, cu[3].y, 2, jevois::yuyv::LightGreen);
250 jevois::rawimage::drawLine(outimg, cu[3].x, cu[3].y, cu[0].x, cu[0].y, 2, jevois::yuyv::LightGreen);
251 jevois::rawimage::drawLine(outimg, cu[4].x, cu[4].y, cu[5].x, cu[5].y, 2, jevois::yuyv::LightGreen);
252 jevois::rawimage::drawLine(outimg, cu[5].x, cu[5].y, cu[6].x, cu[6].y, 2, jevois::yuyv::LightGreen);
253 jevois::rawimage::drawLine(outimg, cu[6].x, cu[6].y, cu[7].x, cu[7].y, 2, jevois::yuyv::LightGreen);
254 jevois::rawimage::drawLine(outimg, cu[7].x, cu[7].y, cu[4].x, cu[4].y, 2, jevois::yuyv::LightGreen);
255 jevois::rawimage::drawLine(outimg, cu[0].x, cu[0].y, cu[4].x, cu[4].y, 2, jevois::yuyv::LightGreen);
256 jevois::rawimage::drawLine(outimg, cu[1].x, cu[1].y, cu[5].x, cu[5].y, 2, jevois::yuyv::LightGreen);
257 jevois::rawimage::drawLine(outimg, cu[2].x, cu[2].y, cu[6].x, cu[6].y, 2, jevois::yuyv::LightGreen);
258 jevois::rawimage::drawLine(outimg, cu[3].x, cu[3].y, cu[7].x, cu[7].y, 2, jevois::yuyv::LightGreen);
259 }
260
261 }
262 }
263
264 if (txtx >=0 && txty >= 0)
265 jevois::rawimage::writeText(outimg, "Detected " + std::to_string(ids.size()) + " ArUco markers.",
266 txtx, txty, jevois::yuyv::White);
267}
268
269#ifdef JEVOIS_PRO
270// ##############################################################################################################
271void ArUco::drawDetections(jevois::GUIhelper & helper, std::vector<int> ids,
272 std::vector<std::vector<cv::Point2f> > corners, std::vector<cv::Vec3d> const & rvecs,
273 std::vector<cv::Vec3d> const & tvecs)
274{
275 ImU32 const col = ImColor(128, 255, 128, 255); // light green for lines
276
277 // This code is like drawDetectedMarkers() in cv::aruco, but for ImGui:
278 int nMarkers = int(corners.size());
279 for (int i = 0; i < nMarkers; ++i)
280 {
281 std::vector<cv::Point2f> const & currentMarker = corners[i];
282
283 // draw marker sides and prepare serial out string:
284 helper.drawPoly(currentMarker, col, true);
285
286 // draw first corner mark
287 helper.drawCircle(currentMarker[0].x, currentMarker[0].y, 3.0F, col, true);
288
289 // draw ID
290 if (ids.empty() == false)
291 {
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);
294 }
295 }
296
297 // This code is like drawAxis() in cv::aruco, but for ImGui:
298 if (dopose::get() && ids.empty() == false)
299 {
300 float const length = markerlen::get() * 0.4F;
301
302 for (size_t i = 0; i < ids.size(); ++i)
303 {
304 // Project axis points:
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));
310
311 std::vector<cv::Point2f> imagePoints;
312 cv::projectPoints(axisPoints, rvecs[i], tvecs[i], itsCalib.camMatrix, itsCalib.distCoeffs, imagePoints);
313
314 // Draw axis lines
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);
318
319 // Also draw a cube if requested:
320 if (showcube::get())
321 {
322 float const len = markerlen::get() * 0.5F;
323
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));
333
334 std::vector<cv::Point2f> cuf;
335 cv::projectPoints(cubePoints, rvecs[i], tvecs[i], itsCalib.camMatrix, itsCalib.distCoeffs, cuf);
336
337 auto drawface =
338 [&](int a, int b, int c, int d)
339 {
340 std::vector<cv::Point2f> p { cuf[a], cuf[b], cuf[c], cuf[d] };
341 helper.drawPoly(p, col, true);
342 };
343
344 // Draw cube lines and faces. For faces, vertices must be in clockwise order:
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);
351 }
352 }
353 }
354
355 helper.itext("Detected " + std::to_string(ids.size()) + " ArUco markers.");
356}
357
358#endif
int h
void detectMarkers(cv::InputArray image, cv::OutputArray ids, cv::OutputArrayOfArrays corners)
Detect markers.
Definition ArUco.C:114
void postUninit() override
Un-initialize, nuke allocated resources.
Definition ArUco.C:105
cv::Ptr< cv::aruco::ArucoDetector > itsDetector
Definition ArUco.H:177
virtual ~ArUco()
Destructor.
Definition ArUco.C:57
jevois::CameraCalibration itsCalib
Definition ArUco.H:178
void drawDetections(jevois::RawImage &outimg, int txtx, int txty, std::vector< int > ids, std::vector< std::vector< cv::Point2f > > corners, std::vector< cv::Vec3d > const &rvecs, std::vector< cv::Vec3d > const &tvecs)
Draw any markers previously detected by detectMarkers()
Definition ArUco.C:164
static cv::aruco::Dictionary getDictionary(aruco::Dict d)
Static method to allow anyone to get the ArUco dictionary for a given value of our dictionary paramet...
Definition ArUco.C:28
void sendSerial(jevois::StdModule *mod, std::vector< int > ids, std::vector< std::vector< cv::Point2f > > corners, unsigned int w, unsigned int h, std::vector< cv::Vec3d > const &rvecs, std::vector< cv::Vec3d > const &tvecs)
Send serial messages about detections.
Definition ArUco.C:127
void postInit() override
Initialize, create the detector and read the config files.
Definition ArUco.C:61
void estimatePoseSingleMarkers(cv::InputArrayOfArrays corners, cv::OutputArray rvecs, cv::OutputArray tvecs)
Estimate pose of individual markers.
Definition ArUco.C:120
void drawCircle(float x, float y, float r, ImU32 col=IM_COL32(128, 255, 128, 255), bool filled=true)
void drawText(float x, float y, char const *txt, ImU32 col=IM_COL32(128, 255, 128, 255))
void drawLine(float x1, float y1, float x2, float y2, ImU32 col=IM_COL32(128, 255, 128, 255))
void itext(char const *txt, ImU32 const &col=IM_COL32_BLACK_TRANS, int line=-1)
void drawPoly(std::vector< cv::Point > const &pts, ImU32 col=IM_COL32(128, 255, 128, 255), bool filled=true)
void sendSerialStd3D(float x, float y, float z, float w=0.0F, float h=0.0F, float d=0.0F, float q1=0.0F, float q2=0.0F, float q3=0.0f, float q4=0.0F, std::string const &id="", std::string const &extra="")
void sendSerialContour2D(unsigned int camw, unsigned int camh, std::vector< cv::Point_< T > > points, std::string const &id="", std::string const &extra="")
#define LERROR(msg)
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
unsigned short constexpr MedPurple
unsigned short constexpr White
unsigned short constexpr MedGreen
unsigned short constexpr MedGrey
unsigned short constexpr LightGreen