Marker.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include "ar_track_alvar/Alvar.h"
25 #include "ar_track_alvar/Marker.h"
26 #include "highgui.h"
27 
28 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::Marker>;
29 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::MarkerData>;
30 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::MarkerArtoolkit>;
31 
32 using namespace std;
33 
34 namespace alvar {
35 using namespace std;
36 
37 #define HEADER_SIZE 8
38 
39 void Marker::VisualizeMarkerPose(IplImage *image, Camera *cam, double visualize2d_points[12][2], CvScalar color) const {
40  // Cube
41  for (int i=0; i<4; i++) {
42  cvLine(image, cvPoint((int)visualize2d_points[i][0], (int)visualize2d_points[i][1]), cvPoint((int)visualize2d_points[(i+1)%4][0], (int)visualize2d_points[(i+1)%4][1]), color);
43  cvLine(image, cvPoint((int)visualize2d_points[i][0], (int)visualize2d_points[i][1]), cvPoint((int)visualize2d_points[4+i][0], (int)visualize2d_points[4+i][1]), color);
44  cvLine(image, cvPoint((int)visualize2d_points[4+i][0], (int)visualize2d_points[4+i][1]), cvPoint((int)visualize2d_points[4+((i+1)%4)][0], (int)visualize2d_points[4+((i+1)%4)][1]), color);
45  }
46  // Coordinates
47  cvLine(image, cvPoint((int)visualize2d_points[8][0], (int)visualize2d_points[8][1]), cvPoint((int)visualize2d_points[9][0], (int)visualize2d_points[9][1]), CV_RGB(255,0,0));
48  cvLine(image, cvPoint((int)visualize2d_points[8][0], (int)visualize2d_points[8][1]), cvPoint((int)visualize2d_points[10][0], (int)visualize2d_points[10][1]), CV_RGB(0,255,0));
49  cvLine(image, cvPoint((int)visualize2d_points[8][0], (int)visualize2d_points[8][1]), cvPoint((int)visualize2d_points[11][0], (int)visualize2d_points[11][1]), CV_RGB(0,0,255));
50 }
51 
52 void Marker::VisualizeMarkerContent(IplImage *image, Camera *cam, double datatext_point[2], double content_point[2]) const {
53 #ifdef VISUALIZE_MARKER_POINTS
54  for (size_t i=0; i<marker_allpoints_img.size(); i++) {
55  if (marker_allpoints_img[i].val == 0)
56  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(0, 255,0));
57  else if (marker_allpoints_img[i].val == 255)
58  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(255, 0,0));
59  else
60  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 2, CV_RGB(255,255,0));
61  }
62 #endif
63 
64  // Marker data
65  CvFont font;
66  cvInitFont(&font, 0, 0.5, 0.5, 0);
67  std::stringstream val;
68  val<<int(GetId());
69  cvPutText(image, val.str().c_str(), cvPoint((int)datatext_point[0], (int)datatext_point[1]), &font, CV_RGB(255,255,0));
70 
71  // MarkerContent
72  int xc = int(content_point[0]);
73  int yc = int(content_point[1]);
74  for (int j=0; j<res*3; j++) {
75  for (int i=0; i<res*3; i++) {
76  int x = xc+i;
77  int y = yc+j;
78  if ((x >= 0) && (x < image->width) &&
79  (y >= 0) && (y < image->height))
80  {
81  if (cvGet2D(marker_content, j/3, i/3).val[0]) {
82  cvSet2D(image, y, x, CV_RGB(255,255,255));
83  } else {
84  cvSet2D(image, y, x, CV_RGB(0,0,0));
85  }
86  }
87  }
88  }
89 }
90 
91 void Marker::VisualizeMarkerError(IplImage *image, Camera *cam, double errortext_point[2]) const {
92  CvFont font;
93  cvInitFont(&font, 0, 0.5, 0.5, 0);
94  std::stringstream val;
95  if (GetError(MARGIN_ERROR|DECODE_ERROR) > 0) {
96  val.str("");
97  val<<int(GetError(MARGIN_ERROR)*100)<<"% ";
98  val<<int(GetError(DECODE_ERROR)*100)<<"% ";
99  cvPutText(image, val.str().c_str(), cvPoint((int)errortext_point[0], (int)errortext_point[1]), &font, CV_RGB(255,0,0));
100  } else if (GetError(TRACK_ERROR) > 0.01) {
101  val.str("");
102  val<<int(GetError(TRACK_ERROR)*100)<<"%";
103  cvPutText(image, val.str().c_str(), cvPoint((int)errortext_point[0], (int)errortext_point[1]), &font, CV_RGB(128,0,0));
104  }
105 }
106 
107 void MarkerData::VisualizeMarkerContent(IplImage *image, Camera *cam, double datatext_point[2], double content_point[2]) const {
108 #ifdef VISUALIZE_MARKER_POINTS
109  for (size_t i=0; i<marker_allpoints_img.size(); i++) {
110  if (marker_allpoints_img[i].val == 0)
111  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(0, 255,0));
112  else if (marker_allpoints_img[i].val == 255)
113  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(255, 0,0));
114  else
115  cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 2, CV_RGB(255,255,0));
116  }
117 #endif
118 
119  // Marker data
120  CvFont font;
121  cvInitFont(&font, 0, 0.5, 0.5, 0);
122  std::stringstream val;
123  CvScalar rgb=CV_RGB(255,255,0);
124  if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
125  val<<int(GetId());
126  } else {
127  if (content_type == MARKER_CONTENT_TYPE_FILE) rgb=CV_RGB(0,255,255);
128  if (content_type == MARKER_CONTENT_TYPE_HTTP) rgb=CV_RGB(255,0,255);
129  val<<data.str;
130  }
131  cvPutText(image, val.str().c_str(), cvPoint((int)datatext_point[0], (int)datatext_point[1]), &font, rgb);
132 }
133 
134 void Marker::Visualize(IplImage *image, Camera *cam, CvScalar color) const {
135  double visualize3d_points[12][3] = {
136  // cube
137  { -(edge_length/2), -(edge_length/2), 0 },
138  { -(edge_length/2), (edge_length/2), 0 },
139  { (edge_length/2), (edge_length/2), 0 },
140  { (edge_length/2), -(edge_length/2), 0 },
141  { -(edge_length/2), -(edge_length/2), edge_length },
142  { -(edge_length/2), (edge_length/2), edge_length },
143  { (edge_length/2), (edge_length/2), edge_length },
144  { (edge_length/2), -(edge_length/2), edge_length },
145  //coordinates
146  { 0, 0, 0 },
147  { edge_length, 0, 0 },
148  { 0, edge_length, 0 },
149  { 0, 0, edge_length },
150  };
151  double visualize2d_points[12][2];
152  CvMat visualize3d_points_mat;
153  CvMat visualize2d_points_mat;
154  cvInitMatHeader(&visualize3d_points_mat, 12, 3, CV_64F, visualize3d_points);
155  cvInitMatHeader(&visualize2d_points_mat, 12, 2, CV_64F, visualize2d_points);
156  cam->ProjectPoints(&visualize3d_points_mat, &pose, &visualize2d_points_mat);
157 
158  VisualizeMarkerPose(image, cam, visualize2d_points, color);
159  VisualizeMarkerContent(image, cam, visualize2d_points[0], visualize2d_points[8]);
160  VisualizeMarkerError(image, cam, visualize2d_points[2]);
161 }
162 
163 void Marker::CompareCorners(vector<PointDouble > &_marker_corners_img, int *orientation, double *error) {
164  vector<PointDouble >::iterator corners_new = _marker_corners_img.begin();
165  vector<PointDouble >::const_iterator corners_old = marker_corners_img.begin();
166  vector<double> errors(4);
167  for (int i=0; i<4; i++) {
168  errors[0] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[i]);
169  errors[1] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+1)%4]);
170  errors[2] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+2)%4]);
171  errors[3] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+3)%4]);
172  }
173  *orientation = min_element(errors.begin(), errors.end()) - errors.begin();
174  *error = sqrt(errors[*orientation]/4);
175  *error /= sqrt(max(PointSquaredDistance(marker_corners_img[0], marker_corners_img[2]), PointSquaredDistance(marker_corners_img[1], marker_corners_img[3])));
176 }
177 
178 void Marker::CompareContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int *orientation) const {
179  // TODO: Note, that to use this method you need to straighten the content
180  // TODO: This method can be used with image based trackingt
181 
182 }
183 
184 bool Marker::UpdateContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
185  return UpdateContentBasic(_marker_corners_img, gray, cam, frame_no);
186 }
187 
188 bool Marker::UpdateContentBasic(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
189  vector<PointDouble > marker_corners_img_undist;
190  marker_corners_img_undist.resize(_marker_corners_img.size());
191  copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img_undist.begin());
192 
193  // Figure out the marker point position in the image
194  Homography H;
195  vector<PointDouble> marker_points_img(marker_points.size());
196  marker_points_img.resize(marker_points.size());
197  cam->Undistort(marker_corners_img_undist);
198  H.Find(marker_corners, marker_corners_img_undist);
199  H.ProjectPoints(marker_points, marker_points_img);
200  cam->Distort(marker_points_img);
201 
202  ros_marker_points_img.clear();
203 
204  // Read the content
205  int x, y;
206  double min = 255.0, max = 0.0;
207  for (int j=0; j<marker_content->height; j++) {
208  for (int i=0; i<marker_content->width; i++) {
209  x = (int)(0.5+Limit(marker_points_img[(j*marker_content->width)+i].x, 1, gray->width-2));
210  y = (int)(0.5+Limit(marker_points_img[(j*marker_content->width)+i].y, 1, gray->height-2));
211 
212  marker_points_img[(j*marker_content->width)+i].val = (int)cvGetReal2D(gray, y, x);
213 
214  ros_marker_points_img.push_back(PointDouble(x,y));
215 
216  /*
217  // Use median of 5 neighbor pixels
218  vector<int> vals;
219  vals.clear();
220  vals.push_back();
221  vals.push_back((int)cvGetReal2D(gray, y-1, x));
222  vals.push_back((int)cvGetReal2D(gray, y, x-1));
223  vals.push_back((int)cvGetReal2D(gray, y+1, x));
224  vals.push_back((int)cvGetReal2D(gray, y, x+1));
225  nth_element(vals.begin(), vals.begin()+2, vals.end());
226  tmp = vals[2];
227  */
228 
229  cvSet2D(marker_content, j, i, cvScalar(marker_points_img[(j*marker_content->width)+i].val));
230  if(marker_points_img[(j*marker_content->width)+i].val > max) max = marker_points_img[(j*marker_content->width)+i].val;
231  if(marker_points_img[(j*marker_content->width)+i].val < min) min = marker_points_img[(j*marker_content->width)+i].val;
232  }
233  }
234 
235  // Take few additional points from border and just
236  // outside the border to make the right thresholding
237  vector<PointDouble> marker_margin_w_img(marker_margin_w.size());
238  vector<PointDouble> marker_margin_b_img(marker_margin_b.size());
239  H.ProjectPoints(marker_margin_w, marker_margin_w_img);
240  H.ProjectPoints(marker_margin_b, marker_margin_b_img);
241  cam->Distort(marker_margin_w_img);
242  cam->Distort(marker_margin_b_img);
243 
244  min = max = 0; // Now min and max values are averages over black and white border pixels.
245  for (size_t i=0; i<marker_margin_w_img.size(); i++) {
246  x = (int)(0.5+Limit(marker_margin_w_img[i].x, 0, gray->width-1));
247  y = (int)(0.5+Limit(marker_margin_w_img[i].y, 0, gray->height-1));
248  marker_margin_w_img[i].val = (int)cvGetReal2D(gray, y, x);
249  max += marker_margin_w_img[i].val;
250  //if(marker_margin_w_img[i].val > max) max = marker_margin_w_img[i].val;
251  //if(marker_margin_w_img[i].val < min) min = marker_margin_w_img[i].val;
252  }
253  for (size_t i=0; i<marker_margin_b_img.size(); i++) {
254  x = (int)(0.5+Limit(marker_margin_b_img[i].x, 0, gray->width-1));
255  y = (int)(0.5+Limit(marker_margin_b_img[i].y, 0, gray->height-1));
256  marker_margin_b_img[i].val = (int)cvGetReal2D(gray, y, x);
257  min += marker_margin_b_img[i].val;
258  //if(marker_margin_b_img[i].val > max) max = marker_margin_b_img[i].val;
259  //if(marker_margin_b_img[i].val < min) min = marker_margin_b_img[i].val;
260  ros_marker_points_img.push_back(PointDouble(x,y));
261  }
262  max /= marker_margin_w_img.size();
263  min /= marker_margin_b_img.size();
264 
265  // Threshold the marker content
266  cvThreshold(marker_content, marker_content, (max+min)/2.0, 255, CV_THRESH_BINARY);
267 
268  // Count erroneous margin nodes
269  int erroneous = 0;
270  int total = 0;
271  for (size_t i=0; i<marker_margin_w_img.size(); i++) {
272  if (marker_margin_w_img[i].val < (max+min)/2.0) erroneous++;
273  total++;
274  }
275  for (size_t i=0; i<marker_margin_b_img.size(); i++) {
276  if (marker_margin_b_img[i].val > (max+min)/2.0) erroneous++;
277  total++;
278  }
279  margin_error = (double)erroneous/total;
280  track_error;
281 
282 #ifdef VISUALIZE_MARKER_POINTS
283  // Now we fill also this temporary debug table for visualizing marker code reading
284  // TODO: this whole vector is only for debug purposes
285  marker_allpoints_img.clear();
286  for (size_t i=0; i<marker_margin_w_img.size(); i++) {
287  PointDouble p = marker_margin_w_img[i];
288  if (p.val < (max+min)/2.0) p.val=255; // error
289  else p.val=0; // ok
290  marker_allpoints_img.push_back(p);
291  }
292  for (size_t i=0; i<marker_margin_b_img.size(); i++) {
293  PointDouble p = marker_margin_b_img[i];
294  if (p.val > (max+min)/2.0) p.val=255; // error
295  else p.val=0; // ok
296  marker_allpoints_img.push_back(p);
297  }
298  for (size_t i=0; i<marker_points_img.size(); i++) {
299  PointDouble p = marker_points_img[i];
300  p.val=128; // Unknown?
301  marker_allpoints_img.push_back(p);
302  }
303 #endif
304  return true;
305 }
306 void Marker::UpdatePose(vector<PointDouble > &_marker_corners_img, Camera *cam, int orientation, int frame_no /* =0 */, bool update_pose /* =true */) {
307  marker_corners_img.resize(_marker_corners_img.size());
308  copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img.begin());
309 
310  // Calculate exterior orientation
311  if(orientation > 0)
312  std::rotate(marker_corners_img.begin(), marker_corners_img.begin() + orientation, marker_corners_img.end());
313 
314  if (update_pose) cam->CalcExteriorOrientation(marker_corners, marker_corners_img, &pose);
315 }
316 bool Marker::DecodeContent(int *orientation) {
317  *orientation = 0;
318  decode_error = 0;
319  return true;
320 }
321 
322 void Marker::SaveMarkerImage(const char *filename, int save_res) const {
323  double scale;
324  if (save_res == 0) {
325  // TODO: More intelligent deduction of a minimum save_res
326  save_res = int((res+margin+margin)*12);
327  }
328  scale = double(save_res)/double(res+margin+margin);
329 
330  IplImage *img = cvCreateImage(cvSize(save_res, save_res), IPL_DEPTH_8U, 1);
331  IplImage *img_content = cvCreateImage(cvSize(int(res*scale+0.5), int(res*scale+0.5)), IPL_DEPTH_8U, 1);
332  cvZero(img);
333  CvMat submat;
334  cvGetSubRect(img, &submat, cvRect(int(margin*scale), int(margin*scale), int(res*scale), int(res*scale)));
335  cvResize(marker_content, img_content, CV_INTER_NN);
336  cvCopy(img_content, &submat);
337  cvSaveImage(filename, img);
338  cvReleaseImage(&img_content);
339  cvReleaseImage(&img);
340 }
341 
342 void Marker::ScaleMarkerToImage(IplImage *image) const {
343  const int multiplier=96;
344  IplImage *img = cvCreateImage(cvSize(int(multiplier*(res+margin+margin)+0.5), int(multiplier*(res+margin+margin)+0.5)), IPL_DEPTH_8U, 1);
345  IplImage *img_content = cvCreateImage(cvSize(int(multiplier*res+0.5), int(multiplier*res+0.5)), IPL_DEPTH_8U, 1);
346  cvZero(img);
347  CvMat submat;
348  cvGetSubRect(img, &submat, cvRect(int(multiplier*margin+0.5), int(multiplier*margin+0.5), int(multiplier*res+0.5), int(multiplier*res+0.5)));
349  cvResize(marker_content, img_content, CV_INTER_NN);
350  cvCopy(img_content, &submat);
351  cvResize(img, image, CV_INTER_NN);
352  cvReleaseImage(&img_content);
353  cvReleaseImage(&img);
354 }
355 
356 void Marker::SetMarkerSize(double _edge_length, int _res, double _margin) {
357  // TODO: Is this right place for default marker size?
358  edge_length = (_edge_length?_edge_length:1);
359  res = _res; //(_res?_res:10);
360  margin = (_margin?_margin:1);
361  double x_min = -0.5*edge_length;
362  double y_min = -0.5*edge_length;
363  double x_max = 0.5*edge_length;
364  double y_max = 0.5*edge_length;
365  double cx_min = (x_min * res)/(res + margin + margin);
366  double cy_min = (y_min * res)/(res + margin + margin);
367  double cx_max = (x_max * res)/(res + margin + margin);
368  double cy_max = (y_max * res)/(res + margin + margin);
369  double step = edge_length / (res + margin + margin);
370 
371  // marker_corners
372  marker_corners_img.resize(4);
373 
374  // Same order as the detected corners
375  marker_corners.clear();
376  marker_corners.push_back(PointDouble(x_min, y_min));
377  marker_corners.push_back(PointDouble(x_max, y_min));
378  marker_corners.push_back(PointDouble(x_max, y_max));
379  marker_corners.push_back(PointDouble(x_min, y_max));
380 
381  // Rest can be done only if we have existing resolution
382  if (res <= 0) return;
383 
384  // marker_points
385  marker_points.clear();
386  for(int j = 0; j < res; ++j) {
387  for(int i = 0; i < res; ++i) {
388  PointDouble pt;
389  pt.y = cy_max - (step*j) - (step/2);
390  pt.x = cx_min + (step*i) + (step/2);
391  marker_points.push_back(pt);
392  }
393  }
394 
395  // Samples to be used in margins
396  // TODO: Now this works only if the "margin" is without decimals
397  // TODO: This should be made a lot cleaner
398  marker_margin_w.clear();
399  marker_margin_b.clear();
400  for(int j = -1; j<=margin-1; j++) {
401  PointDouble pt;
402  // Sides
403  for (int i=0; i<res; i++) {
404  pt.x = cx_min + step*i + step/2;
405  pt.y = y_min + step*j + step/2;
406  if (j < 0) marker_margin_w.push_back(pt);
407  else marker_margin_b.push_back(pt);
408  pt.y = y_max - step*j - step/2;
409  if (j < 0) marker_margin_w.push_back(pt);
410  else marker_margin_b.push_back(pt);
411  pt.y = cy_min + step*i + step/2;
412  pt.x = x_min + step*j + step/2;
413  if (j < 0) marker_margin_w.push_back(pt);
414  else marker_margin_b.push_back(pt);
415  pt.x = x_max - step*j - step/2;
416  if (j < 0) marker_margin_w.push_back(pt);
417  else marker_margin_b.push_back(pt);
418  }
419  // Corners
420  for(int i = -1; i<=margin-1; i++) {
421  pt.x = x_min + step*i + step/2;
422  pt.y = y_min + step*j + step/2;
423  if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
424  else marker_margin_b.push_back(pt);
425  pt.x = x_min + step*i + step/2;
426  pt.y = y_max - step*j - step/2;
427  if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
428  else marker_margin_b.push_back(pt);
429  pt.x = x_max - step*i - step/2;
430  pt.y = y_max - step*j - step/2;
431  if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
432  else marker_margin_b.push_back(pt);
433  pt.x = x_max - step*i - step/2;
434  pt.y = y_min + step*j + step/2;
435  if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
436  else marker_margin_b.push_back(pt);
437  }
438  }
439  /*
440  for(int j = -margin-1; j < res+margin+margin+2; ++j) {
441  for(int i = 0; i < res+margin+margin+2; ++i) {
442  PointDouble pt;
443  pt.y = y_min - step/2 + step*j;
444  pt.x = x_min - step/2 + step*i;
445  if ((pt.x < x_min) || (pt.y < y_min) ||
446  (pt.x > x_max) || (pt.y > y_max))
447  {
448  marker_margin_w.push_back(pt);
449  }
450  else
451  if ((pt.x < cx_min) || (pt.y < cy_min) ||
452  (pt.x > cx_max) || (pt.y > cy_max))
453  {
454  marker_margin_b.push_back(pt);
455  }
456  }
457  }
458  /*
459  //double step = edge_length / (res + margin + margin);
460  for(int j = 0; j < res+margin+margin+2; ++j) {
461  for(int i = 0; i < res+margin+margin+2; ++i) {
462  PointDouble pt;
463  pt.y = y_min - step/2 + step*j;
464  pt.x = x_min - step/2 + step*i;
465  if ((pt.x < x_min) || (pt.y < y_min) ||
466  (pt.x > x_max) || (pt.y > y_max))
467  {
468  marker_margin_w.push_back(pt);
469  }
470  else
471  if ((pt.x < cx_min) || (pt.y < cy_min) ||
472  (pt.x > cx_max) || (pt.y > cy_max))
473  {
474  marker_margin_b.push_back(pt);
475  }
476  }
477  }
478  */
479  /*
480  marker_margin_w.clear();
481  marker_margin_b.clear();
482  for (double y=y_min-(step/2); y<y_max+(step/2); y+=step) {
483  for (double x=x_min-(step/2); x<x_max+(step/2); x+=step) {
484  PointDouble pt(x, y);
485  if ((x < x_min) || (y < y_min) ||
486  (x > x_max) || (y > y_max))
487  {
488  marker_margin_w.push_back(pt);
489  }
490  else
491  if ((x < cx_min) || (y < cy_min) ||
492  (x > cx_max) || (y > cy_max))
493  {
494  marker_margin_b.push_back(pt);
495  }
496  }
497  }
498  */
499  /*
500  marker_points.clear();
501  marker_margin_w.clear();
502  marker_margin_b.clear();
503  for(int j = 0; j < res+margin+margin+2; ++j) {
504  for(int i = 0; i < res+margin+margin+2; ++i) {
505  PointDouble pt;
506  }
507  }
508  */
509 
510  // marker content
511  if (marker_content) cvReleaseMat(&marker_content);
512  marker_content = cvCreateMat(res, res, CV_8U);
513  cvSet(marker_content, cvScalar(255));
514 }
515 Marker::~Marker() {
516  if (marker_content) cvReleaseMat(&marker_content);
517 }
518 Marker::Marker(double _edge_length, int _res, double _margin)
519 {
520  marker_content = NULL;
521  margin_error = 0;
522  decode_error = 0;
523  track_error = 0;
524  SetMarkerSize(_edge_length, _res, _margin);
525  ros_orientation = -1;
526  ros_corners_3D.resize(4);
527  valid=false;
528 }
529 Marker::Marker(const Marker& m) {
530  marker_content = NULL;
531  SetMarkerSize(m.edge_length, m.res, m.margin);
532 
533  pose = m.pose;
534  margin_error = m.margin_error;
535  decode_error = m.decode_error;
536  track_error = m.track_error;
537  cvCopy(m.marker_content, marker_content);
538  ros_orientation = m.ros_orientation;
539 
540  ros_marker_points_img.resize(m.ros_marker_points_img.size());
541  copy(m.ros_marker_points_img.begin(), m.ros_marker_points_img.end(), ros_marker_points_img.begin());
542  marker_corners.resize(m.marker_corners.size());
543  copy(m.marker_corners.begin(), m.marker_corners.end(), marker_corners.begin());
544  marker_points.resize(m.marker_points.size());
545  copy(m.marker_points.begin(), m.marker_points.end(), marker_points.begin());
546  marker_corners_img.resize(m.marker_corners_img.size());
547  copy(m.marker_corners_img.begin(), m.marker_corners_img.end(), marker_corners_img.begin());
548  ros_corners_3D.resize(m.ros_corners_3D.size());
549  copy(m.ros_corners_3D.begin(), m.ros_corners_3D.end(), ros_corners_3D.begin());
550 
551  valid = m.valid;
552 #ifdef VISUALIZE_MARKER_POINTS
553  marker_allpoints_img.resize(m.marker_allpoints_img.size());
554  copy(m.marker_allpoints_img.begin(), m.marker_allpoints_img.end(), marker_allpoints_img.begin());
555 #endif
556 }
557 
558 bool MarkerArtoolkit::DecodeContent(int *orientation) {
559  int a = (int)cvGetReal2D(marker_content, 0, 0);
560  int b = (int)cvGetReal2D(marker_content, res-1, 0);
561  int c = (int)cvGetReal2D(marker_content, res-1, res-1);
562  int d = (int)cvGetReal2D(marker_content, 0, res-1);
563  if (!a && !b && c) *orientation = 0;
564  else if (!b && !c && d) *orientation = 1;
565  else if (!c && !d && a) *orientation = 2;
566  else if (!d && !a && b) *orientation = 3;
567  else return false;
568 
569  Bitset bs;
570  bs.clear();
571  for (int j = 0; j < res; j++) {
572  for (int i = 0; i < res ; i++) {
573  if (*orientation == 0) {
574  if ((j == 0) && (i == 0)) continue;
575  if ((j == res-1) && (i == 0)) continue;
576  if ((j == res-1) && (i == res-1)) continue;
577  if (cvGetReal2D(marker_content, j, i)) bs.push_back(false);
578  else bs.push_back(true);
579  }
580  else if (*orientation == 1) {
581  if (((res-i-1) == res-1) && (j == 0)) continue;
582  if (((res-i-1) == res-1) && (j == res-1)) continue;
583  if (((res-i-1) == 0) && (j == res-1)) continue;
584  if (cvGetReal2D(marker_content, res-i-1, j)) bs.push_back(false);
585  else bs.push_back(true);
586  }
587  else if (*orientation == 2) {
588  if (((res-j-1) == res-1) && ((res-i-1) == res-1)) continue;
589  if (((res-j-1) == 0) && ((res-i-1) == res-1)) continue;
590  if (((res-j-1) == 0) && ((res-i-1) == 0)) continue;
591  if (cvGetReal2D(marker_content, res-j-1, res-i-1)) bs.push_back(false);
592  else bs.push_back(true);
593  }
594  else if (*orientation == 3) {
595  if ((i == 0) && ((res-j-1) == res-1)) continue;
596  if ((i == 0) && ((res-j-1) == 0)) continue;
597  if ((i == res-1) && ((res-j-1) == 0)) continue;
598  if (cvGetReal2D(marker_content, i, res-j-1)) bs.push_back(false);
599  else bs.push_back(true);
600  }
601  }
602  }
603  id = bs.ulong();
604  return true;
605 }
606 
607 void MarkerArtoolkit::SetContent(unsigned long _id) {
608  // Fill in the content values
609  margin_error = 0;
610  decode_error = 0;
611  id = _id;
612  Bitset bs;
613  bs.push_back_meaningful(_id);
614  for (int j = res-1; j >= 0; j--) {
615  for (int i = res-1; i >= 0 ; i--) {
616  if ((j == 0) && (i == 0))
617  cvSetReal2D(marker_content, j, i, 0);
618  else if ((j == res-1) && (i == 0))
619  cvSetReal2D(marker_content, j, i, 0);
620  else if ((j == res-1) && (i == res-1))
621  cvSetReal2D(marker_content, j, i, 255);
622  else {
623  if (bs.Length() && bs.pop_back())
624  cvSetReal2D(marker_content, j, i, 0);
625  else
626  cvSetReal2D(marker_content, j, i, 255);
627  }
628  }
629  }
630 }
631 
632 void MarkerData::DecodeOrientation(int *error, int *total, int *orientation) {
633  int i,j;
634  vector<double> errors(4);
635  int color = 255;
636 
637  // Resolution identification
638  j = res/2;
639  for (i=0; i<res; i++) {
640  (*total)++;
641  if ((int)cvGetReal2D(marker_content, j, i) != color) errors[0]++;
642  if ((int)cvGetReal2D(marker_content, i, j) != color) errors[1]++;
643  color = (color?0:255);
644  }
645  errors[2] = errors[0];
646  errors[3] = errors[1];
647 
648  // Orientation identification
649  i = res/2;
650  for (j = (res/2)-2; j <= (res/2)+2; j++) {
651  if (j < (res/2)) {
652  (*total)++;
653  if ((int)cvGetReal2D(marker_content, j, i) != 0) errors[0]++;
654  if ((int)cvGetReal2D(marker_content, i, j) != 0) errors[1]++;
655  if ((int)cvGetReal2D(marker_content, j, i) != 255) errors[2]++;
656  if ((int)cvGetReal2D(marker_content, i, j) != 255) errors[3]++;
657  } else if (j > (res/2)) {
658  (*total)++;
659  if ((int)cvGetReal2D(marker_content, j, i) != 255) errors[0]++;
660  if ((int)cvGetReal2D(marker_content, i, j) != 255) errors[1]++;
661  if ((int)cvGetReal2D(marker_content, j, i) != 0) errors[2]++;
662  if ((int)cvGetReal2D(marker_content, i, j) != 0) errors[3]++;
663  }
664  }
665  *orientation = min_element(errors.begin(), errors.end()) - errors.begin();
666  *error = int(errors[*orientation]);
667  //*orientation = 0; // ttehop
668 }
669 
670 bool MarkerData::DetectResolution(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam) {
671  vector<PointDouble> marker_corners_img_undist;
672  marker_corners_img_undist.resize(_marker_corners_img.size());
673  copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img_undist.begin());
674 
675  // line_points
676  std::vector<PointDouble> line_points;
677  PointDouble pt;
678  line_points.clear();
679  pt.x=0; pt.y=0; line_points.push_back(pt);
680  pt.x=-0.5*edge_length; pt.y=0; line_points.push_back(pt);
681  pt.x=+0.5*edge_length; pt.y=0; line_points.push_back(pt);
682  pt.x=0; pt.y=-0.5*edge_length; line_points.push_back(pt);
683  pt.x=0; pt.y=+0.5*edge_length; line_points.push_back(pt);
684 
685  // Figure out the marker point position in the image
686  // TODO: Note that line iterator cannot iterate outside image
687  // therefore we need to distort the endpoints and iterate straight lines.
688  // Right way would be to iterate undistorted lines and distort line points.
689  Homography H;
690  vector<PointDouble> line_points_img(line_points.size());
691  line_points_img.resize(line_points.size());
692  cam->Undistort(marker_corners_img_undist);
693  H.Find(marker_corners, marker_corners_img_undist);
694  H.ProjectPoints(line_points, line_points_img);
695  cam->Distort(line_points_img);
696 
697  // Now we have undistorted line end points
698  // Find lines and then distort the whole line
699  int white_count[4] = {0}; // white counts for lines 1->0, 2->0, 3->0, 4->0
700  CvPoint pt1, pt2;
701  pt2.x = int(line_points_img[0].x);
702  pt2.y = int(line_points_img[0].y);
703  if ((pt2.x < 0) || (pt2.y < 0) ||
704  (pt2.x >= gray->width) || (pt2.y >= gray->height))
705  {
706  return false;
707  }
708  bool white=true;
709  for (int i=0; i<4; i++) {
710  CvLineIterator iterator;
711  pt1.x = int(line_points_img[i+1].x);
712  pt1.y = int(line_points_img[i+1].y);
713  if ((pt1.x < 0) || (pt1.y < 0) ||
714  (pt1.x >= gray->width) || (pt1.y >= gray->height))
715  {
716  return false;
717  }
718  int count = cvInitLineIterator(gray, pt1, pt2, &iterator, 8, 0);
719  std::vector<uchar> vals;
720  for(int ii = 0; ii < count; ii++ ){
721  CV_NEXT_LINE_POINT(iterator);
722  vals.push_back(*(iterator.ptr));
723  }
724  uchar vmin = *(std::min_element(vals.begin(), vals.end()));
725  uchar vmax = *(std::max_element(vals.begin(), vals.end()));
726  uchar thresh = (vmin+vmax)/2;
727  white=true;
728  int bc=0, wc=0, N=2;
729  for (size_t ii=0; ii<vals.size(); ii++) {
730  // change the color status if we had
731  // N subsequent pixels of the other color
732  if (vals[ii] < thresh) { bc++; wc=0; }
733  else { wc++; bc=0; }
734 
735  if (white && (bc >= N)) {
736  white=false;
737  }
738  else if (!white && (wc >= N)) {
739  white=true;
740  white_count[i]++;
741  }
742  }
743  }
744 
745  if ((white_count[0]+white_count[1]) == (white_count[2]+white_count[3])) return false;
746  else if ((white_count[0]+white_count[1]) > (white_count[2]+white_count[3])) {
747  if (white_count[0] != white_count[1]) return false;
748  if (white_count[0] < 2) return false;
749  int nof_whites = white_count[0]*2-(white?1:0); // 'white' contains middle color
750  int new_res = 2*nof_whites-1;
751  SetMarkerSize(edge_length, new_res, margin);
752  }
753  else {
754  if (white_count[2] != white_count[3]) return false;
755  if (white_count[2] < 2) return false;
756  if (((white_count[2]%2) == 0) != white) return false;
757  int nof_whites = white_count[2]*2-(white?1:0);
758  int new_res = 2*nof_whites-1;
759  SetMarkerSize(edge_length, new_res, margin);
760  }
761  return true;
762 }
763 
764 bool MarkerData::UpdateContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
765  if (res == 0) {
766  if (!DetectResolution(_marker_corners_img, gray, cam)) return false;
767  }
768  return UpdateContentBasic(_marker_corners_img, gray, cam, frame_no);
769 }
770 
771 int MarkerData::DecodeCode(int orientation, BitsetExt *bs, int *erroneous, int *total,
772  unsigned char* content_type)
773 {
774  // TODO: The orientation isn't fully understood?
775  //for (int j = res-1; j >= 0; j--) {
776  for (int j = 0; j < res; j++) {
777  for (int i = 0; i < res ; i++) {
778  // TODO: Does this work ok for larger markers?
779  if ((orientation == 0) || (orientation == 2)) {
780  if (j == res/2) continue;
781  if ((i == res/2) && (j >= (res/2)-2) && (j <= (res/2)+2)) continue;
782  } else {
783  if (i == res/2) continue;
784  if ((j == res/2) && (i >= (res/2)-2) && (i <= (res/2)+2)) continue;
785  }
786  int color = 0;
787  if (orientation == 0) color = (int)cvGetReal2D(marker_content, j, i);
788  else if (orientation == 1) color = (int)cvGetReal2D(marker_content, res-i-1, j);
789  else if (orientation == 2) color = (int)cvGetReal2D(marker_content, res-j-1, res-i-1);
790  else if (orientation == 3) color = (int)cvGetReal2D(marker_content, i, res-j-1);
791  if (color) bs->push_back(false);
792  else bs->push_back(true);
793  (*total)++;
794  }
795  }
796 
797  unsigned char flags = 0;
798  int errors = 0;
799 
800  // if we have larger than 16-bit code, then we have a header; 16-bit code has a
801  // hamming(8,4) coded number
802  if(bs->Length() > 16){
803  // read header (8-bit hamming(8,4) -> 4-bit flags)
804  BitsetExt header;
805 
806  for(int i = 0; i < HEADER_SIZE; i++)
807  header.push_back(bs->pop_front());
808 
809  errors = header.hamming_dec(8);
810  if(errors == -1){
811  //OutputDebugString("header decoding failed!!!!!\n");
812  return errors;
813  }
814 
815  flags = header.uchar();
816  }
817  else
818  flags &= MARKER_CONTENT_TYPE_NUMBER;
819 
820  // check which hamming we are using
821  //bs->Output(cout); cout<<endl;
822  if(flags & 0x8) errors = bs->hamming_dec(16);
823  else errors = bs->hamming_dec(8);
824  *content_type = flags & 0x7;
825 
826  if (errors > 0) (*erroneous) += errors;
827  return errors;
828 }
829 void MarkerData::Read6bitStr(BitsetExt *bs, char *s, size_t s_max_len) {
830  deque<bool> bits = bs->GetBits();
831  deque<bool>::const_iterator iter;
832  size_t len = 0;
833  int bitpos = 5;
834  unsigned long c=0;
835  for (iter = bits.begin(); iter != bits.end(); iter++) {
836  if (*iter) c |= (0x01 << bitpos);
837  bitpos--;
838  if (bitpos < 0) {
839  if (c == 000) s[len] = ':';
840  else if ((c >= 001) && (c <= 032)) s[len] = 'a' + (char)c - 1;
841  else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 1;
842  else if (c == 045) s[len] = '+';
843  else if (c == 046) s[len] = '-';
844  else if (c == 047) s[len] = '*';
845  else if (c == 050) s[len] = '/';
846  else if (c == 051) s[len] = '(';
847  else if (c == 052) s[len] = ')';
848  else if (c == 053) s[len] = '$';
849  else if (c == 054) s[len] = '=';
850  else if (c == 055) s[len] = ' ';
851  else if (c == 056) s[len] = ',';
852  else if (c == 057) s[len] = '.';
853  else if (c == 060) s[len] = '#';
854  else if (c == 061) s[len] = '[';
855  else if (c == 062) s[len] = ']';
856  else if (c == 063) s[len] = '%';
857  else if (c == 064) s[len] = '\"';
858  else if (c == 065) s[len] = '_';
859  else if (c == 066) s[len] = '!';
860  else if (c == 067) s[len] = '&';
861  else if (c == 070) s[len] = '\'';
862  else if (c == 071) s[len] = '?';
863  else if (c == 072) s[len] = '<';
864  else if (c == 073) s[len] = '>';
865  else if (c == 074) s[len] = '@';
866  else if (c == 075) s[len] = '\\';
867  else if (c == 076) s[len] = '^';
868  else if (c == 077) s[len] = ';';
869  else s[len] = '?';
870  len++;
871  if (len >= (s_max_len-1)) break;
872  bitpos=5; c=0;
873  }
874  }
875  s[len] = 0;
876 }
877 
878 bool MarkerData::DecodeContent(int *orientation) {
879  //bool decode(vector<int>& colors, int *orientation, double *error) {
880  *orientation = 0;
881 
882  BitsetExt bs;
883  int erroneous=0;
884  int total=0;
885 
886  DecodeOrientation(&erroneous, &total, orientation);
887  int err = DecodeCode(*orientation, &bs, &erroneous, &total, &content_type);
888  if(err == -1) {
889  // couldn't fix
890  decode_error = DBL_MAX;
891  return false;
892  }
893 
894  if(content_type == MARKER_CONTENT_TYPE_NUMBER){
895  data.id = bs.ulong();
896  }
897  else {
898  Read6bitStr(&bs, data.str, MAX_MARKER_STRING_LEN);
899  }
900 
901  decode_error = (double)(erroneous)/total;
902 
903  return true;
904 }
905 
906 void MarkerData::Add6bitStr(BitsetExt *bs, char *s) {
907  while (*s) {
908  unsigned char c = (unsigned char)*s;
909  if (c == ':') bs->push_back((unsigned char)0,6);
910  else if ((c >= 'A') && (c <= 'Z')) bs->push_back((unsigned char)(001 + c - 'A'),6);
911  else if ((c >= 'a') && (c <= 'z')) bs->push_back((unsigned char)(001 + c - 'a'),6);
912  else if ((c >= '0') && (c <= '9')) bs->push_back((unsigned char)(033 + c - '0'),6);
913  else if (c == '+') bs->push_back((unsigned char)045,6);
914  else if (c == '-') bs->push_back((unsigned char)046,6);
915  else if (c == '*') bs->push_back((unsigned char)047,6);
916  else if (c == '/') bs->push_back((unsigned char)050,6);
917  else if (c == '(') bs->push_back((unsigned char)051,6);
918  else if (c == ')') bs->push_back((unsigned char)052,6);
919  else if (c == '$') bs->push_back((unsigned char)053,6);
920  else if (c == '=') bs->push_back((unsigned char)054,6);
921  else if (c == ' ') bs->push_back((unsigned char)055,6);
922  else if (c == ',') bs->push_back((unsigned char)056,6);
923  else if (c == '.') bs->push_back((unsigned char)057,6);
924  else if (c == '#') bs->push_back((unsigned char)060,6);
925  else if (c == '[') bs->push_back((unsigned char)061,6);
926  else if (c == ']') bs->push_back((unsigned char)062,6);
927  else if (c == '%') bs->push_back((unsigned char)063,6);
928  else if (c == '\"') bs->push_back((unsigned char)064,6);
929  else if (c == '_') bs->push_back((unsigned char)065,6);
930  else if (c == '!') bs->push_back((unsigned char)066,6);
931  else if (c == '&') bs->push_back((unsigned char)067,6);
932  else if (c == '\'') bs->push_back((unsigned char)070,6);
933  else if (c == '?') bs->push_back((unsigned char)071,6);
934  else if (c == '<') bs->push_back((unsigned char)072,6);
935  else if (c == '>') bs->push_back((unsigned char)073,6);
936  else if (c == '@') bs->push_back((unsigned char)074,6);
937  else if (c == '\\') bs->push_back((unsigned char)075,6);
938  else if (c == '^') bs->push_back((unsigned char)076,6);
939  else if (c == ';') bs->push_back((unsigned char)077,6);
940  else bs->push_back((unsigned char)071,6);
941  s++;
942  }
943 }
944 
945 int MarkerData::UsableDataBits(int marker_res, int hamming) {
946  if (marker_res < 5) return 0;
947  if (!(marker_res % 2)) return 0;
948  int bits = marker_res * marker_res;
949  if (marker_res > 5) bits -= 8; // With larger resolutions we reserve 8 bits for hamming(8,4) encoded 4 flags
950  bits -= marker_res; // center line indicating the resolution
951  bits -= 4; // the four pixels indicating the orientation
952  int tail = bits % hamming;
953  if (tail < 3) bits -= tail; // hamming can't use tail pixels if there is only 2 or 1 of them
954  return bits;
955 }
956 
957 void MarkerData::SetContent(MarkerContentType _content_type, unsigned long _id, const char *_str, bool force_strong_hamming, bool verbose) {
958  // Fill in the content values
959  content_type = _content_type;
960  margin_error = 0;
961  decode_error = 0;
962  if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
963  data.id = _id;
964  } else {
965  STRCPY(data.str, MAX_MARKER_STRING_LEN, _str);
966  }
967  // Encode
968  const int max_marker_res = 127;
969  BitsetExt bs_flags(verbose);
970  BitsetExt bs_data(verbose);
971  int enc_bits; // How many encoded bits fits in the marker
972  int data_bits; // How many data bits fit inside the encoded bits
973  int hamming; // Do we use 8-bit or 16-bit hamming?
974  if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
975  bs_data.push_back_meaningful(data.id);
976  for (res=5; res<max_marker_res; res+=2) {
977  hamming = 8;
978  enc_bits = UsableDataBits(res, hamming);
979  data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
980  if (data_bits >= bs_data.Length()) break;
981  if ((res > 5) && !force_strong_hamming) {
982  hamming = 16;
983  enc_bits = UsableDataBits(res, hamming);
984  data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
985  if (data_bits >= bs_data.Length()) break;
986  }
987  }
988  bs_data.fill_zeros_left(data_bits);
989  bs_data.hamming_enc(hamming);
990  if (verbose) {
991  cout<<"Using hamming("<<hamming<<") for "<<res<<"x"<<res<<" marker"<<endl;
992  cout<<bs_data.Length()<<" bits are filled into "<<data_bits;
993  cout<<" bits, and encoded into "<<enc_bits<<" bits"<<endl;
994  cout<<"data src: "; bs_data.Output(cout); cout<<endl;
995  cout<<"data enc: "; bs_data.Output(cout); cout<<endl;
996  }
997  if (res > 5) {
998  if (hamming == 16) bs_flags.push_back(true);
999  else bs_flags.push_back(false);
1000  bs_flags.push_back((unsigned long)0,3);
1001  bs_flags.hamming_enc(8);
1002  if (verbose) {
1003  cout<<"flags src: "; bs_flags.Output(cout); cout<<endl;
1004  cout<<"flags enc: "; bs_flags.Output(cout); cout<<endl;
1005  }
1006  }
1007  } else {
1008  Add6bitStr(&bs_data, data.str);
1009  for (res=7; res<max_marker_res; res+=2) {
1010  hamming = 8;
1011  enc_bits = UsableDataBits(res, hamming);
1012  data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
1013  if (data_bits >= bs_data.Length()) break;
1014  if (!force_strong_hamming) {
1015  hamming = 16;
1016  enc_bits = UsableDataBits(res, hamming);
1017  data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
1018  if (data_bits >= bs_data.Length()) break;
1019  }
1020  }
1021  while (bs_data.Length() < ((data_bits/6)*6)) {
1022  bs_data.push_back((unsigned char)055,6); // Add space
1023  }
1024  while (bs_data.Length() < data_bits) {
1025  bs_data.push_back(false); // Add 0
1026  }
1027  bs_data.hamming_enc(hamming);
1028  if (hamming == 16) bs_flags.push_back(true);
1029  else bs_flags.push_back(false);
1030  if (content_type == MARKER_CONTENT_TYPE_STRING) bs_flags.push_back((unsigned long)1,3);
1031  else if (content_type == MARKER_CONTENT_TYPE_FILE) bs_flags.push_back((unsigned long)2,3);
1032  else if (content_type == MARKER_CONTENT_TYPE_HTTP) bs_flags.push_back((unsigned long)3,3);
1033  bs_flags.hamming_enc(8);
1034  if (verbose) {
1035  cout<<"Using hamming("<<hamming<<") for "<<res<<"x"<<res<<" marker"<<endl;
1036  cout<<bs_data.Length()<<" bits are filled into "<<data_bits;
1037  cout<<" bits, and encoded into "<<enc_bits<<" bits"<<endl;
1038  cout<<"data src: "; bs_data.Output(cout); cout<<endl;
1039  cout<<"data enc: "; bs_data.Output(cout); cout<<endl;
1040  cout<<"flags src: "; bs_flags.Output(cout); cout<<endl;
1041  cout<<"flags enc: "; bs_flags.Output(cout); cout<<endl;
1042  }
1043  }
1044 
1045  // Fill in the marker content
1046  deque<bool> bs(bs_flags.GetBits());
1047  bs.insert(bs.end(), bs_data.GetBits().begin(), bs_data.GetBits().end());
1048  deque<bool>::const_iterator iter = bs.begin();
1049  SetMarkerSize(edge_length, res, margin);
1050  cvSet(marker_content, cvScalar(255));
1051  for (int j=0; j<res; j++) {
1052  for (int i=0; i<res; i++) {
1053  if (j == res/2) {
1054  if (i%2) cvSet2D(marker_content, j, i, cvScalar(0));
1055  } else if ((i == res/2) && (j < res/2) && (j >= (res/2)-2)) {
1056  cvSet2D(marker_content, j, i, cvScalar(0));
1057  } else if ((i == res/2) && (j > res/2) && (j <= (res/2)+2)) {
1058  cvSet2D(marker_content, j, i, cvScalar(255));
1059  } else {
1060  if (iter != bs.end()) {
1061  if (*iter) cvSet2D(marker_content, j, i, cvScalar(0));
1062  iter++;
1063  }
1064  }
1065  }
1066  }
1067 }
1068 
1069 } // namespace alvar
alvar::Bitset::clear
void clear()
Clear the bits.
Definition: Bitset.cpp:43
alvar
Main ALVAR namespace.
Definition: Alvar.h:174
alvar::Marker::ros_orientation
int ros_orientation
Definition: Marker.h:181
alvar::Bitset::pop_back
bool pop_back()
Pop the back bit.
Definition: Bitset.cpp:89
alvar::Marker::marker_corners_img
std::vector< PointDouble > marker_corners_img
Marker corners in image coordinates.
Definition: Marker.h:177
alvar::Homography
Simple Homography class for finding and projecting points between two planes.
Definition: Camera.h:273
margin
const double margin
Definition: SampleMarkerHide.cpp:11
image
unsigned char * image
Definition: GlutViewer.cpp:155
font
CvFont font
Definition: SampleTrack.cpp:12
alvar::Bitset::ulong
unsigned long ulong()
The Bitset as &#39;unsigned long&#39;.
Definition: Bitset.cpp:120
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
HEADER_SIZE
#define HEADER_SIZE
Definition: Marker.cpp:37
height
int height
Definition: GlutViewer.cpp:160
alvar::Bitset::Output
std::ostream & Output(std::ostream &os) const
Output the bits to selected ostream.
Definition: Bitset.cpp:34
alvar::Camera
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
alvar::Bitset::pop_front
bool pop_front()
Pop the front bit.
Definition: Bitset.cpp:83
alvar::Marker::decode_error
double decode_error
Definition: Marker.h:163
alvar::Bitset::fill_zeros_left
void fill_zeros_left(const size_t bit_count)
Fill the Bitset with non-meaningful zeros.
Definition: Bitset.cpp:69
alvar::Marker::pose
Pose pose
The current marker Pose.
Definition: Marker.h:136
alvar::Bitset
Bitset is a basic class for handling bit sequences
Definition: Bitset.h:62
alvar::Camera::ProjectPoints
void ProjectPoints(std::vector< CvPoint3D64f > &pw, Pose *pose, std::vector< CvPoint2D64f > &pi) const
Project points.
alvar::Homography::Find
void Find(const std::vector< PointDouble > &pw, const std::vector< PointDouble > &pi)
Find Homography for two point-sets.
Definition: Camera.cpp:838
alvar::Bitset::push_back_meaningful
void push_back_meaningful(const unsigned long l)
Push back meaningful bits from &#39;long&#39; l.
Definition: Bitset.cpp:61
width
int width
Definition: GlutViewer.cpp:159
ar_track_alvar
Definition: kinect_filtering.h:63
alvar::Marker::valid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool valid
Definition: Marker.h:62
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
alvar::Marker::ros_marker_points_img
std::vector< PointDouble > ros_marker_points_img
Marker points in image coordinates.
Definition: Marker.h:179
alvar::Marker
Basic 2D Marker functionality.
Definition: Marker.h:52
gray
cv::Mat gray
Definition: SampleMarkerlessDetector.cpp:15
alvar::Marker::track_error
double track_error
Definition: Marker.h:164
alvar::PointSquaredDistance
double PointSquaredDistance(PointType p1, PointType p2)
Returns the squared distance of two points.
Definition: Util.h:116
alvar::Marker::edge_length
double edge_length
Definition: Marker.h:165
d
Drawable d[32]
Definition: SampleMarkerDetector.cpp:11
alvar::Bitset::uchar
unsigned char uchar()
The Bitset as &#39;unsigned char&#39;.
Definition: Bitset.cpp:131
alvar::Bitset::Length
int Length()
The length of the Bitset.
Definition: Bitset.cpp:31
ALVAR_EXPORT
#define ALVAR_EXPORT
Definition: Alvar.h:168
alvar::Bitset::GetBits
std::deque< bool > & GetBits()
The Bitset as &#39;deque<bool>&#39;.
Definition: Bitset.h:121
step
unsigned int step
alvar::Marker::marker_points
std::vector< PointDouble > marker_points
Marker color points in marker coordinates.
Definition: Marker.h:173
alvar::Camera::Distort
void Distort(CvPoint2D32f &point)
Applys the lens distortion for one point on an image plane.
Definition: Camera.cpp:611
alvar::Marker::marker_content
CvMat * marker_content
Definition: Marker.h:168
alvar::BitsetExt::hamming_dec
int hamming_dec(int block_len)
Hamming decoding &#39;in-place&#39; using the defined block length.
Definition: Bitset.cpp:319
alvar::PointDouble
ALVAR_EXPORT Point< CvPoint2D64f > PointDouble
The default double point type.
Definition: Util.h:108
min
int min(int a, int b)
alvar::MarkerIteratorImpl
Iterator implementation for traversing templated Marker vector without the template.
Definition: Marker.h:294
alvar::Homography::ProjectPoints
void ProjectPoints(const std::vector< PointDouble > &from, std::vector< PointDouble > &to)
Project points using the Homography.
Definition: Camera.cpp:868
alvar::STRCPY
void STRCPY(char *to, size_t size, const char *src)
Definition: Util.h:298
alvar::Camera::CalcExteriorOrientation
void CalcExteriorOrientation(std::vector< CvPoint3D64f > &pw, std::vector< CvPoint2D64f > &pi, Pose *pose)
Calculate exterior orientation.
alvar::Bitset::push_back
void push_back(const bool bit)
Push back one bit.
Definition: Bitset.cpp:44
cam
Camera * cam
Definition: FindMarkerBundles.cpp:85
alvar::Marker::margin_error
double margin_error
Definition: Marker.h:162
res
const int res
Definition: SampleFilter.cpp:13
alvar::BitsetExt
An extended Bitset ( BitsetExt ) for handling e.g. Hamming encoding/decoding.
Definition: Bitset.h:135
Marker.h
This file implements a marker interface as well as ALVAR markers and ARToolKit markers.
alvar::Marker::margin
double margin
Definition: Marker.h:167
Alvar.h
This file defines library export definitions, version numbers and build information.
alvar::Limit
double ALVAR_EXPORT Limit(double val, double min_val, double max_val)
Limits a number to between two values.
Definition: Util.cpp:241
alvar::Marker::ros_corners_3D
ar_track_alvar::ARCloud ros_corners_3D
Definition: Marker.h:180
alvar::Camera::Undistort
void Undistort(std::vector< PointDouble > &points)
Unapplys the lens distortion for points on image plane.
alvar::Marker::marker_corners
std::vector< PointDouble > marker_corners
Marker corners in marker coordinates.
Definition: Marker.h:175
alvar::BitsetExt::hamming_enc
void hamming_enc(int block_len)
Hamming encoding &#39;in-place&#39; using the defined block length.
Definition: Bitset.cpp:312

ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:04