JeVoisBase  1.20
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
ARtoolkit.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: Shixian Wen - 3641 Watt Way, HNB-10A - Los Angeles, BA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - shixianw@usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
20 #include <jevois/Core/Module.H>
21 #include <opencv2/objdetect.hpp>
22 #include <opencv2/imgproc.hpp>
23 
24 // ##############################################################################################################
26 { }
27 
28 // ##############################################################################################################
30 {
31  // Defer reading camera parameters to first processed frame, so we know the resolution:
33 }
34 
35 // ##############################################################################################################
37 {
38  // clean all of the initialization:
39  if (markersSquare)
40  {
42 
43  // Tracking cleanup.
44  if (arPattHandle)
45  {
46  arPattDetach(arHandle);
47  arPattDeleteHandle(arPattHandle);
48  }
49  ar3DDeleteHandle(&ar3DHandle);
50  arDeleteHandle(arHandle);
51  arParamLTFree(&gCparamLT);
52  }
53 
55 }
56 
57 // ##############################################################################################################
58 void ARtoolkit::manualinit(int w, int h, AR_PIXEL_FORMAT pixformat)
59 {
60  std::string markerConfigDataFilename;
61 
62  switch (artoolkit::dictionary::get())
63  {
64  case artoolkit::Dict::AR_MATRIX_CODE_3x3: markerConfigDataFilename = "markers0.dat"; break;
65  case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63: markerConfigDataFilename = "markers1.dat"; break;
66  case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65: markerConfigDataFilename = "markers2.dat"; break;
67  default: markerConfigDataFilename = "markers2.dat";
68  }
69 
70  ARParam cparam;
71 
72  arParamChangeSize(&cparam, w, h, &cparam);
73 
74  std::string const CPARA_NAME =
75  absolutePath(camparams::get() + std::to_string(w) + 'x' + std::to_string(h) + ".dat");
76 
77  if (arParamLoad(absolutePath(CPARA_NAME).c_str(), 1, &cparam) < 0)
78  LERROR("Failed to load camera parameters " << CPARA_NAME << " -- IGNORED");
79 
80  if ((gCparamLT = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == nullptr) LFATAL("Error in arParamLTCreate");
81 
82  if ((arHandle = arCreateHandle(gCparamLT)) == nullptr) LFATAL("Error in arCreateHandle");
83 
84  if ((ar3DHandle = ar3DCreateHandle(&cparam)) == nullptr) LFATAL("Error in ar3DCreateHandle");
85 
86  if (arSetPixelFormat(arHandle, pixformat) < 0) LFATAL("Error in arSetPixelFormat");
87 
88  if ((arPattHandle = arPattCreateHandle()) == nullptr) LFATAL("Error in arPattCreateHandle");
89 
90  newMarkers(absolutePath(markerConfigDataFilename).c_str(), arPattHandle, &markersSquare,
92 
93  arPattAttach(arHandle, arPattHandle);
94 
95  arSetPatternDetectionMode(arHandle, AR_MATRIX_CODE_DETECTION);
96 
97  switch (dictionary::get())
98  {
99  case artoolkit::Dict::AR_MATRIX_CODE_3x3:
100  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3); break;
101  case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63:
102  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_HAMMING63); break;
103  case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65:
104  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_PARITY65); break;
105  default: arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_PARITY65);
106  }
107 
108  itsW = w; itsH = h;
109 
110  // Set the artoolkit thresh_mode:
111  AR_LABELING_THRESH_MODE modea;
112  switch (artoolkit::threshmode::get())
113  {
114  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_MANUAL:
115  modea = AR_LABELING_THRESH_MODE_MANUAL; break;
116  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_MEDIAN:
117  modea = AR_LABELING_THRESH_MODE_AUTO_MEDIAN; break;
118  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_OTSU:
119  modea = AR_LABELING_THRESH_MODE_AUTO_OTSU; break;
120  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE:
121  modea = AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE; break;
122  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_BRACKETING:
123  modea = AR_LABELING_THRESH_MODE_AUTO_BRACKETING; break;
124  default: modea = AR_LABELING_THRESH_MODE_AUTO_OTSU;
125  }
126  arSetLabelingThreshMode(arHandle, modea);
127 }
128 
129 // ##############################################################################################################
131 {
132  // Finalize initialization now that image format and size is known:
133  if (arHandle == nullptr)
134  {
135  switch (image.fmt)
136  {
137  case V4L2_PIX_FMT_YUYV: manualinit(image.width, image.height, AR_PIXEL_FORMAT_yuvs); break;
138  case V4L2_PIX_FMT_GREY: manualinit(image.width, image.height, AR_PIXEL_FORMAT_MONO); break;
139  case V4L2_PIX_FMT_RGB565: manualinit(image.width, image.height, AR_PIXEL_FORMAT_RGB_565); break;
140  case V4L2_PIX_FMT_BGR24: manualinit(image.width, image.height, AR_PIXEL_FORMAT_BGR); break;
141  default: LFATAL("Unsupported image format, should be V4L2_PIX_FMT_YUYV, V4L2_PIX_FMT_GREY, "
142  "V4L2_PIX_FMT_RGB565, or V4L2_PIX_FMT_BGR24");
143  }
144  }
145 
146  // Not sure why arDetectMarker() needs write access to the pixels? input pixels should be const...
147  detectInternal(image.pixels<unsigned char>());
148 }
149 
150 // ##############################################################################################################
151 void ARtoolkit::detectMarkers(cv::Mat const & image)
152 {
153  // Finalize initialization now that image format and size is known:
154  if (arHandle == nullptr)
155  {
156  switch (image.type())
157  {
158  case CV_8UC3: manualinit(image.cols, image.rows, AR_PIXEL_FORMAT_BGR); break;
159  case CV_8UC1: manualinit(image.cols, image.rows, AR_PIXEL_FORMAT_MONO); break;
160  default: LFATAL("Unsupported image format, should be CV_8UC3 for BGR or CV_8UC1 for gray");
161  }
162  }
163 
164  detectInternal(image.data);
165 }
166 
167 // ##############################################################################################################
168 void ARtoolkit::detectInternal(unsigned char const * data)
169 {
170  itsResults.clear();
171 
172  // Not sure why arDetectMarker() needs write access to the pixels? input pixels should be const...
173  if (arDetectMarker(arHandle, const_cast<unsigned char *>(data)) < 0)
174  { LERROR("Error trying to detect markers -- IGNORED"); return; }
175 
176  double const confidence_thresh = artoolkit::confthresh::get();
177  int const numDetected = arGetMarkerNum(arHandle);
178  int const useContPoseEstimation = artoolkit::contpose::get();
179 
180  // Validate each detection:
181  ARMarkerInfo * markerInfo = arGetMarker(arHandle);
182 
183  for (int i = 0; i < markersSquareCount; ++i)
184  {
186  int k = -1;
187  if (markersSquare[i].patt_type == AR_PATTERN_TYPE_MATRIX)
188  {
189  for (int j = 0; j < numDetected; ++j)
190  {
191  if (markersSquare[i].patt_id == markerInfo[j].id)
192  {
193  if (k == -1)
194  {
195  if (markerInfo[j].cf >= confidence_thresh) k = j; // First marker detected.
196  }
197  else if (markerInfo[j].cf > markerInfo[k].cf) k = j; // Higher confidence marker detected.
198  }
199  }
200  }
201 
202  // Picked up the candidate markerInfo[k], need to verify confidence level here
203  if (k != -1)
204  {
206  result.id = markerInfo[k].id;
207  result.width = markersSquare[i].marker_width;
208  result.height = markersSquare[i].marker_height;
209  result.p2d[0] = markerInfo[k].pos[0]; result.p2d[1] = markerInfo[k].pos[1];
210 
211  // get the transformation matrix from the markers to camera in camera coordinate
212  // rotation matrix + translation matrix
213  ARdouble err;
214  if (markersSquare[i].validPrev && useContPoseEstimation)
215  err = arGetTransMatSquareCont(ar3DHandle, &(markerInfo[k]), markersSquare[i].trans,
216  markersSquare[i].marker_width, markersSquare[i].trans);
217  else
218  err = arGetTransMatSquare(ar3DHandle, &(markerInfo[k]), markersSquare[i].marker_width, markersSquare[i].trans);
219  if (err > 1.0) continue; // forget about this one if we cannot properly recover the 3D matrix
220 
221  arUtilMat2QuatPos(markersSquare[i].trans, result.q, result.pos);
222 
223  for (int i1 = 0; i1 < 4; ++i1)
224  {
225  auto const & v1 = markerInfo[k].vertex[i1];
226  result.corners.push_back(cv::Point(int(v1[0] + 0.5F), int(v1[1] + 0.5F)));
227  }
228 
229  itsResults.push_back(result);
230  markersSquare[i].valid = TRUE;
231  }
232  else markersSquare[i].valid = FALSE;
233  }
234 }
235 
236 // ##############################################################################################################
237 void ARtoolkit::drawDetections(jevois::RawImage & outimg, int txtx, int txty)
238 {
239  for (arresults const & r : itsResults)
240  {
241  jevois::rawimage::drawCircle(outimg, r.p2d[0], r.p2d[1], 3, 2, jevois::yuyv::LightPink);
242 
243  for (int i = 0; i < 4; ++i)
244  {
245  auto const & v1 = r.corners[i];
246  auto const & v2 = r.corners[(i + 1) % 4];
247  jevois::rawimage::drawLine(outimg, v1.x, v1.y, v2.x, v2.y, 2, jevois::yuyv::LightPink);
248  }
249 
250  jevois::rawimage::writeText(outimg, "AR=" + std::to_string(r.id), r.p2d[0]+5, r.p2d[1]+5, jevois::yuyv::LightPink);
251  }
252 
253  // Show number of good detections:
254  if (txtx >= 0 && txty >= 0)
255  jevois::rawimage::writeText(outimg, "Detected " + std::to_string(itsResults.size()) + " ARtoolkit markers.",
256  txtx, txty, jevois::yuyv::White);
257 }
258 
259 // ##############################################################################################################
260 #ifdef JEVOIS_PRO
262 {
263  static ImU32 const col = ImColor(255, 128, 128, 255); // light pink
264 
265  for (arresults const & r : itsResults)
266  {
267  helper.drawCircle(r.p2d[0], r.p2d[1], 3.0F, col, true);
268 
269  std::vector<cv::Point2f> p;
270  for (int i = 0; i < 4; ++i) p.emplace_back(cv::Point2f(r.corners[i].x, r.corners[i].y));
271  helper.drawPoly(p, col, true);
272 
273  helper.drawText(r.p2d[0]+5, r.p2d[1]+5, ("AR=" + std::to_string(r.id)).c_str(), col);
274  }
275 
276  helper.itext("Detected " + std::to_string(itsResults.size()) + " ARtoolkit markers.");
277 }
278 #endif
279 
280 // ##############################################################################################################
282 {
283  if (msg3d::get())
284  for (arresults const & r : itsResults)
285  mod->sendSerialStd3D(r.pos[0], r.pos[1], r.pos[2], // position
286  r.width, r.height, 1.0F, // size
287  r.q[0], r.q[1], r.q[2], r.q[3], // pose
288  "A" + std::to_string(r.id)); // decoded ID with "A" prefix for ARtoolkit
289  else
290  for (arresults const & r : itsResults)
291  mod->sendSerialContour2D(itsW, itsH, r.corners, "A" + std::to_string(r.id));
292 }
293 
jevois::GUIhelper
_ARMarkerSquare::marker_width
ARdouble marker_width
Definition: ARMarkerSquare.h:86
jevois::StdModule::sendSerialContour2D
void sendSerialContour2D(unsigned int camw, unsigned int camh, std::vector< cv::Point_< T > > points, std::string const &id="", std::string const &extra="")
ARtoolkit::manualinit
void manualinit(int w, int h, AR_PIXEL_FORMAT pixformat)
Manual initialization which should be run on first frame once its size is known.
Definition: ARtoolkit.C:58
jevois::GUIhelper::drawCircle
void drawCircle(float x, float y, float r, ImU32 col=IM_COL32(128, 255, 128, 255), bool filled=true)
ARtoolkit::drawDetections
void drawDetections(jevois::RawImage &outimg, int txtx=-1, int txty=-1)
Draw any markers previously detected by detectMarkers()
Definition: ARtoolkit.C:237
AR_PATTERN_TYPE_MATRIX
#define AR_PATTERN_TYPE_MATRIX
Definition: ARMarkerSquare.h:101
jevois::GUIhelper::itext
void itext(char const *txt, ImU32 const &col=IM_COL32_BLACK_TRANS, int line=-1)
Module.H
deleteMarkers
void deleteMarkers(ARMarkerSquare **markersSquare_p, int *markersSquareCount_p, ARPattHandle *arPattHandle)
Definition: ARMarkerSquare.c:242
ARtoolkit::postUninit
void postUninit() override
Definition: ARtoolkit.C:36
ARtoolkit::markersSquareCount
int markersSquareCount
Definition: ARtoolkit.H:157
jevois::RawImage
ARtoolkit::markersSquare
ARMarkerSquare * markersSquare
Definition: ARtoolkit.H:156
ARtoolkit::gCparamLT
ARParamLT * gCparamLT
Definition: ARtoolkit.H:155
jevois::rawimage::drawCircle
void drawCircle(RawImage &img, int x, int y, unsigned int rad, unsigned int thick, unsigned int col)
ARtoolkit::gARPattDetectionMode
int gARPattDetectionMode
Definition: ARtoolkit.H:153
_ARMarkerSquare::marker_height
ARdouble marker_height
Definition: ARMarkerSquare.h:87
demo.result
result
Definition: demo.py:74
LERROR
#define LERROR(msg)
ARtoolkit::itsW
unsigned int itsW
Definition: ARtoolkit.H:158
demo.image
image
Definition: demo.py:84
ARtoolkit::itsH
unsigned int itsH
Definition: ARtoolkit.H:158
jevois::rawimage::writeText
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
F
float F
ARtoolkit::itsResults
std::vector< arresults > itsResults
Results of the last detection round.
Definition: ARtoolkit.H:139
jevois::GUIhelper::drawPoly
void drawPoly(std::vector< cv::Point > const &pts, ImU32 col=IM_COL32(128, 255, 128, 255), bool filled=true)
ARtoolkit::sendSerial
void sendSerial(jevois::StdModule *mod)
Send serial messages about detections.
Definition: ARtoolkit.C:281
ARtoolkit::arPattHandle
ARPattHandle * arPattHandle
Definition: ARtoolkit.H:152
LFATAL
#define LFATAL(msg)
ARtoolkit::arresults
Definition: ARtoolkit.H:128
jevois::GUIhelper::drawText
void drawText(float x, float y, char const *txt, ImU32 col=IM_COL32(128, 255, 128, 255))
_ARMarkerSquare::validPrev
bool validPrev
Definition: ARMarkerSquare.h:83
RawImageOps.H
ARtoolkit::~ARtoolkit
~ARtoolkit()
Destructor.
Definition: ARtoolkit.C:25
ARtoolkit::detectInternal
void detectInternal(unsigned char const *data)
Definition: ARtoolkit.C:168
to_string
std::string to_string(T const &val)
unFreeze
void unFreeze()
ARtoolkit::detectMarkers
void detectMarkers(jevois::RawImage const &image)
Detect markers in an image.
Definition: ARtoolkit.C:130
jevois::Component::absolutePath
std::filesystem::path absolutePath(std::filesystem::path const &path="")
newMarkers
void newMarkers(const char *markersConfigDataFilePathC, ARPattHandle *arPattHandle, ARMarkerSquare **markersSquare_out, int *markersSquareCount_out, int *patternDetectionMode_out)
Definition: ARMarkerSquare.c:89
ARtoolkit::postInit
void postInit() override
Core initialization.
Definition: ARtoolkit.C:29
ARtoolkit::ar3DHandle
AR3DHandle * ar3DHandle
Definition: ARtoolkit.H:154
h
int h
jevois::StdModule::sendSerialStd3D
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="")
ARtoolkit::arHandle
ARHandle * arHandle
Definition: ARtoolkit.H:151
freeze
void freeze()
jevois::StdModule
jevois::rawimage::drawLine
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
_ARMarkerSquare::valid
bool valid
Definition: ARMarkerSquare.h:82
ARtoolkit.H
demo.w
w
Definition: demo.py:85