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