JeVoisBase  1.8
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 //using namespace std;
22 // ##############################################################################################################
24 { }
25 
26 // ##############################################################################################################
28 {
29  // Defer reading camera parameters to first processed frame, so we know the resolution:
31 
32 }
33 
34 // ##############################################################################################################
36 {
37  // clean all of the initialization:
38  if (markersSquare)
39  {
41 
42  // Tracking cleanup.
43  if (arPattHandle)
44  {
45  arPattDetach(arHandle);
46  arPattDeleteHandle(arPattHandle);
47  }
48  ar3DDeleteHandle(&ar3DHandle);
49  arDeleteHandle(arHandle);
50  arParamLTFree(&gCparamLT);
51  }
52 
54 }
55 
56 // ##############################################################################################################
57 void ARtoolkit::manualinit(int w, int h, AR_PIXEL_FORMAT pixformat)
58 {
59  std::string markerConfigDataFilename;
60 
61  switch (artoolkit::dictionary::get())
62  {
63  case artoolkit::Dict::AR_MATRIX_CODE_3x3: markerConfigDataFilename = "markers0.dat"; break;
64  case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63: markerConfigDataFilename = "markers1.dat"; break;
65  case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65: markerConfigDataFilename = "markers2.dat"; break;
66  default: markerConfigDataFilename = "markers2.dat";
67  }
68 
69  ARParam cparam;
70 
71  arParamChangeSize(&cparam, w, h, &cparam);
72 
73  std::string const CPARA_NAME =
74  absolutePath(camparams::get() + std::to_string(w) + 'x' + std::to_string(h) + ".dat");
75 
76  if (arParamLoad(absolutePath(CPARA_NAME).c_str(), 1, &cparam) < 0)
77  LFATAL("Failed to load camera parameters " << CPARA_NAME);
78 
79  if ((gCparamLT = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == nullptr) LFATAL("Error in arParamLTCreate");
80 
81  if ((arHandle = arCreateHandle(gCparamLT)) == nullptr) LFATAL("Error in arCreateHandle");
82 
83  if ((ar3DHandle = ar3DCreateHandle(&cparam)) == nullptr) LFATAL("Error in ar3DCreateHandle");
84 
85  if (arSetPixelFormat(arHandle, pixformat) < 0) LFATAL("Error in arSetPixelFormat");
86 
87  if ((arPattHandle = arPattCreateHandle()) == nullptr) LFATAL("Error in arPattCreateHandle");
88 
89  newMarkers(absolutePath(markerConfigDataFilename).c_str(), arPattHandle, &markersSquare,
91 
92  arPattAttach(arHandle, arPattHandle);
93 
94  arSetPatternDetectionMode(arHandle, AR_MATRIX_CODE_DETECTION);
95 
96  switch (dictionary::get())
97  {
98  case artoolkit::Dict::AR_MATRIX_CODE_3x3:
99  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3); break;
100  case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63:
101  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_HAMMING63); break;
102  case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65:
103  arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_PARITY65); break;
104  default: arSetMatrixCodeType(arHandle, AR_MATRIX_CODE_3x3_PARITY65);
105  }
106 
107  itsW = w; itsH = h;
108 
109  // Set the artoolkit thresh_mode:
110  AR_LABELING_THRESH_MODE modea;
111  switch (artoolkit::threshmode::get())
112  {
113  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_MANUAL:
114  modea = AR_LABELING_THRESH_MODE_MANUAL; break;
115  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_MEDIAN:
116  modea = AR_LABELING_THRESH_MODE_AUTO_MEDIAN; break;
117  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_OTSU:
118  modea = AR_LABELING_THRESH_MODE_AUTO_OTSU; break;
119  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE:
120  modea = AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE; break;
121  case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_BRACKETING:
122  modea = AR_LABELING_THRESH_MODE_AUTO_BRACKETING; break;
123  default: modea = AR_LABELING_THRESH_MODE_AUTO_OTSU;
124  }
125  arSetLabelingThreshMode(arHandle, modea);
126 }
127 
128 // ##############################################################################################################
130 {
131  // Finalize initialization now that image format and size is known:
132  if (arHandle == nullptr)
133  {
134  switch (image.fmt)
135  {
136  case V4L2_PIX_FMT_YUYV: manualinit(image.width, image.height, AR_PIXEL_FORMAT_yuvs); break;
137  case V4L2_PIX_FMT_GREY: manualinit(image.width, image.height, AR_PIXEL_FORMAT_MONO); break;
138  case V4L2_PIX_FMT_RGB565: manualinit(image.width, image.height, AR_PIXEL_FORMAT_RGB_565); break;
139  case V4L2_PIX_FMT_BGR24: manualinit(image.width, image.height, AR_PIXEL_FORMAT_BGR); break;
140  default: LFATAL("Unsupported image format, should be V4L2_PIX_FMT_YUYV, V4L2_PIX_FMT_GREY, "
141  "V4L2_PIX_FMT_RGB565, or V4L2_PIX_FMT_BGR24");
142  }
143  }
144 
145  // Not sure why arDetectMarker() needs write access to the pixels? input pixels should be const...
146  detectInternal(image.pixels<unsigned char>());
147 }
148 
149 // ##############################################################################################################
150 void ARtoolkit::detectMarkers(cv::Mat const & image)
151 {
152  // Finalize initialization now that image format and size is known:
153  if (arHandle == nullptr)
154  {
155  switch (image.type())
156  {
157  case CV_8UC3: manualinit(image.cols, image.rows, AR_PIXEL_FORMAT_BGR); break;
158  case CV_8UC1: manualinit(image.cols, image.rows, AR_PIXEL_FORMAT_MONO); break;
159  default: LFATAL("Unsupported image format, should be CV_8UC3 for BGR or CV_8UC1 for gray");
160  }
161  }
162 
163  detectInternal(image.data);
164 }
165 
166 // ##############################################################################################################
167 void ARtoolkit::detectInternal(unsigned char const * data)
168 {
169  itsResults.clear();
170 
171  // Not sure why arDetectMarker() needs write access to the pixels? input pixels should be const...
172  if (arDetectMarker(arHandle, const_cast<unsigned char *>(data)) < 0)
173  { LERROR("Error trying to detect markers -- IGNORED"); return; }
174 
175  double const confidence_thresh = artoolkit::confthresh::get();
176  int const numDetected = arGetMarkerNum(arHandle);
177  int const useContPoseEstimation = artoolkit::contpose::get();
178 
179  // Validate each detection:
180  ARMarkerInfo * markerInfo = arGetMarker(arHandle);
181 
182  for (int i = 0; i < markersSquareCount; ++i)
183  {
185  int k = -1;
186  if (markersSquare[i].patt_type == AR_PATTERN_TYPE_MATRIX)
187  {
188  for (int j = 0; j < numDetected; ++j)
189  {
190  if (markersSquare[i].patt_id == markerInfo[j].id)
191  {
192  if (k == -1)
193  {
194  if (markerInfo[j].cf >= confidence_thresh) k = j; // First marker detected.
195  }
196  else if (markerInfo[j].cf > markerInfo[k].cf) k = j; // Higher confidence marker detected.
197  }
198  }
199  }
200  // picked up the candidate markerInfo[k], I need to verify the k's confidence level at 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 // ##############################################################################################################
236 void ARtoolkit::drawDetections(jevois::RawImage & outimg, int txtx, int txty)
237 {
238  for (arresults const & r : itsResults)
239  {
240  jevois::rawimage::drawCircle(outimg, r.p2d[0], r.p2d[1], 3, 2, jevois::yuyv::LightPink);
241 
242  for (int i = 0; i < 4; ++i)
243  {
244  auto const & v1 = r.corners[i];
245  auto const & v2 = r.corners[(i + 1) % 4];
246  jevois::rawimage::drawLine(outimg, v1.x, v1.y, v2.x, v2.y, 2, jevois::yuyv::LightPink);
247  }
248 
249  jevois::rawimage::writeText(outimg, "AR=" + std::to_string(r.id), r.p2d[0]+5, r.p2d[1]+5, jevois::yuyv::LightPink);
250  }
251 
252  // Show number of good detections:
253  if (txtx >= 0 && txty >= 0)
254  jevois::rawimage::writeText(outimg, "Detected " + std::to_string(itsResults.size()) + " ARtoolkit markers.",
255  txtx, txty, jevois::yuyv::White);
256 }
257 
258 // ##############################################################################################################
260 {
261  if (msg3d::get())
262  for (arresults const & r : itsResults)
263  mod->sendSerialStd3D(r.pos[0], r.pos[1], r.pos[2], // position
264  r.width, r.height, 1.0F, // size
265  r.q[0], r.q[1], r.q[2], r.q[3], // pose
266  "A" + std::to_string(r.id)); // decoded ID with "A" prefix for ARtoolkit
267  else
268  for (arresults const & r : itsResults)
269  mod->sendSerialContour2D(itsW, itsH, r.corners, "A" + std::to_string(r.id));
270 }
271 
void sendSerialContour2D(unsigned int camw, unsigned int camh, std::vector< cv::Point_< T > > points, std::string const &id="", std::string const &extra="")
void unFreeze()
void drawCircle(RawImage &img, int x, int y, unsigned int rad, unsigned int thick, unsigned int col)
ARdouble marker_width
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
unsigned int height
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:57
ARdouble marker_height
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
unsigned int fmt
void sendSerial(jevois::StdModule *mod)
Send serial messages about detections.
Definition: ARtoolkit.C:259
int id
Marker ID.
Definition: ARtoolkit.H:125
void newMarkers(const char *markersConfigDataFilePathC, ARPattHandle *arPattHandle, ARMarkerSquare **markersSquare_out, int *markersSquareCount_out, int *patternDetectionMode_out)
#define LERROR(msg)
void deleteMarkers(ARMarkerSquare **markersSquare_p, int *markersSquareCount_p, ARPattHandle *arPattHandle)
void detectMarkers(jevois::RawImage const &image)
Detect markers in an image.
Definition: ARtoolkit.C:129
ARPattHandle * arPattHandle
Definition: ARtoolkit.H:147
unsigned int itsW
Definition: ARtoolkit.H:153
void postInit() override
Core initialization.
Definition: ARtoolkit.C:27
AR3DHandle * ar3DHandle
Definition: ARtoolkit.H:149
unsigned int itsH
Definition: ARtoolkit.H:153
ARHandle * arHandle
Definition: ARtoolkit.H:146
int markersSquareCount
Definition: ARtoolkit.H:152
double q[4]
quaternion
Definition: ARtoolkit.H:128
~ARtoolkit()
Destructor.
Definition: ARtoolkit.C:23
ARMarkerSquare * markersSquare
Definition: ARtoolkit.H:151
void freeze()
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="")
#define LFATAL(msg)
int p2d[2]
2d position in image coords
Definition: ARtoolkit.H:130
double pos[3]
3d position
Definition: ARtoolkit.H:129
std::string to_string(T const &val)
void drawDetections(jevois::RawImage &outimg, int txtx=-1, int txty=-1)
Draw any markers previously detected by detectMarkers()
Definition: ARtoolkit.C:236
std::vector< arresults > itsResults
Results of the last detection round.
Definition: ARtoolkit.H:134
double width
size in mm
Definition: ARtoolkit.H:126
T const * pixels() const
double height
size in mm
Definition: ARtoolkit.H:127
void detectInternal(unsigned char const *data)
Definition: ARtoolkit.C:167
#define AR_PATTERN_TYPE_MATRIX
std::vector< cv::Point > corners
corners in standardized image coords
Definition: ARtoolkit.H:131
void postUninit() override
Definition: ARtoolkit.C:35
unsigned int width
std::string absolutePath(std::string const &path="")
ARParamLT * gCparamLT
Definition: ARtoolkit.H:150
int gARPattDetectionMode
Definition: ARtoolkit.H:148