Marker.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "Alvar.h"
00025 #include "Marker.h"
00026 #include "highgui.h"
00027 
00028 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::Marker>;
00029 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::MarkerData>;
00030 template class ALVAR_EXPORT alvar::MarkerIteratorImpl<alvar::MarkerArtoolkit>;
00031 
00032 using namespace std;
00033 
00034 namespace alvar {
00035 using namespace std;
00036 
00037 #define HEADER_SIZE 8
00038 
00039 void Marker::VisualizeMarkerPose(IplImage *image, Camera *cam, double visualize2d_points[12][2], CvScalar color) const {
00040         // Cube
00041         for (int i=0; i<4; i++) {
00042                 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);
00043                 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);
00044                 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);
00045         }
00046         // Coordinates
00047         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));
00048         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));
00049         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));
00050 }
00051 
00052 void Marker::VisualizeMarkerContent(IplImage *image, Camera *cam, double datatext_point[2], double content_point[2]) const {
00053 #ifdef VISUALIZE_MARKER_POINTS
00054         for (size_t i=0; i<marker_allpoints_img.size(); i++) {
00055                 if (marker_allpoints_img[i].val == 0) 
00056                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(0, 255,0));
00057                 else if (marker_allpoints_img[i].val == 255) 
00058                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(255, 0,0));
00059                 else 
00060                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 2, CV_RGB(255,255,0));
00061         }
00062 #endif
00063 
00064         // Marker data
00065         CvFont font;
00066         cvInitFont(&font, 0, 0.5, 0.5, 0);
00067         std::stringstream val;
00068         val<<int(GetId());
00069         cvPutText(image, val.str().c_str(), cvPoint((int)datatext_point[0], (int)datatext_point[1]), &font, CV_RGB(255,255,0));
00070 
00071         // MarkerContent
00072         int xc = int(content_point[0]);
00073         int yc = int(content_point[1]);
00074         for (int j=0; j<res*3; j++) {
00075                 for (int i=0; i<res*3; i++) {
00076                         int x = xc+i;
00077                         int y = yc+j;
00078                         if ((x >= 0) && (x < image->width) &&
00079                                 (y >= 0) && (y < image->height))
00080                         {
00081                                 if (cvGet2D(marker_content, j/3, i/3).val[0]) {
00082                                         cvSet2D(image, y, x, CV_RGB(255,255,255));
00083                                 } else {
00084                                         cvSet2D(image, y, x, CV_RGB(0,0,0));
00085                                 }
00086                         }
00087                 }
00088         }
00089 }
00090 
00091 void Marker::VisualizeMarkerError(IplImage *image, Camera *cam, double errortext_point[2]) const {
00092         CvFont font;
00093         cvInitFont(&font, 0, 0.5, 0.5, 0);
00094         std::stringstream val;
00095         if (GetError(MARGIN_ERROR|DECODE_ERROR) > 0) {
00096                 val.str("");
00097                 val<<int(GetError(MARGIN_ERROR)*100)<<"% ";
00098                 val<<int(GetError(DECODE_ERROR)*100)<<"% ";
00099                 cvPutText(image, val.str().c_str(), cvPoint((int)errortext_point[0], (int)errortext_point[1]), &font, CV_RGB(255,0,0));
00100         } else if (GetError(TRACK_ERROR) > 0.01) {
00101                 val.str("");
00102                 val<<int(GetError(TRACK_ERROR)*100)<<"%";
00103                 cvPutText(image, val.str().c_str(), cvPoint((int)errortext_point[0], (int)errortext_point[1]), &font, CV_RGB(128,0,0));
00104         }
00105 }
00106 
00107 void MarkerData::VisualizeMarkerContent(IplImage *image, Camera *cam, double datatext_point[2], double content_point[2]) const {
00108 #ifdef VISUALIZE_MARKER_POINTS
00109         for (size_t i=0; i<marker_allpoints_img.size(); i++) {
00110                 if (marker_allpoints_img[i].val == 0) 
00111                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(0, 255,0));
00112                 else if (marker_allpoints_img[i].val == 255) 
00113                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 1, CV_RGB(255, 0,0));
00114                 else 
00115                         cvCircle(image, cvPoint(int(marker_allpoints_img[i].x), int(marker_allpoints_img[i].y)), 2, CV_RGB(255,255,0));
00116         }
00117 #endif
00118 
00119         // Marker data
00120         CvFont font;
00121         cvInitFont(&font, 0, 0.5, 0.5, 0);
00122         std::stringstream val;
00123         CvScalar rgb=CV_RGB(255,255,0);
00124         if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
00125                 val<<int(GetId());
00126         } else {
00127                 if (content_type == MARKER_CONTENT_TYPE_FILE) rgb=CV_RGB(0,255,255);
00128                 if (content_type == MARKER_CONTENT_TYPE_HTTP) rgb=CV_RGB(255,0,255);
00129                 val<<data.str;
00130         }
00131         cvPutText(image, val.str().c_str(), cvPoint((int)datatext_point[0], (int)datatext_point[1]), &font, rgb);
00132 }
00133 
00134 void Marker::Visualize(IplImage *image, Camera *cam, CvScalar color) const {
00135         double visualize3d_points[12][3] = {
00136                 // cube
00137                 { -(edge_length/2), -(edge_length/2), 0 },
00138                 { -(edge_length/2),  (edge_length/2), 0 },
00139                 {  (edge_length/2),  (edge_length/2), 0 },
00140                 {  (edge_length/2), -(edge_length/2), 0 },
00141                 { -(edge_length/2), -(edge_length/2), edge_length },
00142                 { -(edge_length/2),  (edge_length/2), edge_length },
00143                 {  (edge_length/2),  (edge_length/2), edge_length },
00144                 {  (edge_length/2), -(edge_length/2), edge_length },
00145                 //coordinates
00146                 {  0, 0, 0 },
00147                 {  edge_length, 0, 0 },
00148                 {  0, edge_length, 0 },
00149                 {  0, 0, edge_length },
00150         };
00151         double visualize2d_points[12][2];
00152         CvMat visualize3d_points_mat;
00153         CvMat visualize2d_points_mat;
00154         cvInitMatHeader(&visualize3d_points_mat, 12, 3, CV_64F, visualize3d_points);
00155         cvInitMatHeader(&visualize2d_points_mat, 12, 2, CV_64F, visualize2d_points);
00156         cam->ProjectPoints(&visualize3d_points_mat, &pose, &visualize2d_points_mat);
00157 
00158         VisualizeMarkerPose(image, cam, visualize2d_points, color);
00159         VisualizeMarkerContent(image, cam, visualize2d_points[0], visualize2d_points[8]);
00160         VisualizeMarkerError(image, cam, visualize2d_points[2]);
00161 }
00162 
00163 void Marker::CompareCorners(vector<PointDouble > &_marker_corners_img, int *orientation, double *error) {
00164         vector<PointDouble >::iterator corners_new = _marker_corners_img.begin();
00165         vector<PointDouble >::const_iterator corners_old = marker_corners_img.begin();
00166         vector<double> errors(4);
00167         for (int i=0; i<4; i++) {
00168                 errors[0] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[i]);
00169                 errors[1] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+1)%4]);
00170                 errors[2] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+2)%4]);
00171                 errors[3] += PointSquaredDistance(marker_corners_img[i], _marker_corners_img[(i+3)%4]);
00172         }
00173         *orientation = min_element(errors.begin(), errors.end()) - errors.begin();
00174         *error = sqrt(errors[*orientation]/4);
00175         *error /= sqrt(max(PointSquaredDistance(marker_corners_img[0], marker_corners_img[2]), PointSquaredDistance(marker_corners_img[1], marker_corners_img[3])));
00176 }
00177 
00178 void Marker::CompareContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int *orientation) const {
00179         // TODO: Note, that to use this method you need to straighten the content
00180         // TODO: This method can be used with image based trackingt
00181 
00182 }
00183 
00184 bool Marker::UpdateContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
00185         return UpdateContentBasic(_marker_corners_img, gray, cam, frame_no);
00186 }
00187 
00188 bool Marker::UpdateContentBasic(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
00189         vector<PointDouble > marker_corners_img_undist;
00190         marker_corners_img_undist.resize(_marker_corners_img.size());
00191         copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img_undist.begin());
00192 
00193         // Figure out the marker point position in the image
00194         Homography H;
00195         vector<PointDouble> marker_points_img(marker_points.size());
00196         marker_points_img.resize(marker_points.size());
00197         cam->Undistort(marker_corners_img_undist);
00198         H.Find(marker_corners, marker_corners_img_undist);
00199         H.ProjectPoints(marker_points, marker_points_img);
00200         cam->Distort(marker_points_img);
00201         
00202         ros_marker_points_img.clear();
00203 
00204     // Read the content
00205     int x, y;
00206         double min = 255.0, max = 0.0;
00207         for (int j=0; j<marker_content->height; j++) {
00208                 for (int i=0; i<marker_content->width; i++) {
00209                         x = (int)(0.5+Limit(marker_points_img[(j*marker_content->width)+i].x, 1, gray->width-2));
00210                         y = (int)(0.5+Limit(marker_points_img[(j*marker_content->width)+i].y, 1, gray->height-2));
00211                         
00212                         marker_points_img[(j*marker_content->width)+i].val = (int)cvGetReal2D(gray, y, x);
00213                         
00214                         ros_marker_points_img.push_back(PointDouble(x,y));
00215 
00216                         /*
00217                         // Use median of 5 neighbor pixels
00218                         vector<int> vals;
00219                         vals.clear();
00220                         vals.push_back();
00221                         vals.push_back((int)cvGetReal2D(gray, y-1, x));
00222                         vals.push_back((int)cvGetReal2D(gray, y, x-1));
00223                         vals.push_back((int)cvGetReal2D(gray, y+1, x));
00224                         vals.push_back((int)cvGetReal2D(gray, y, x+1));
00225                         nth_element(vals.begin(), vals.begin()+2, vals.end());
00226                         tmp = vals[2];
00227                         */
00228 
00229                         cvSet2D(marker_content, j, i, cvScalar(marker_points_img[(j*marker_content->width)+i].val));
00230                         if(marker_points_img[(j*marker_content->width)+i].val > max) max = marker_points_img[(j*marker_content->width)+i].val;
00231                         if(marker_points_img[(j*marker_content->width)+i].val < min) min = marker_points_img[(j*marker_content->width)+i].val;
00232                 }
00233         }
00234 
00235         // Take few additional points from border and just 
00236         // outside the border to make the right thresholding
00237         vector<PointDouble> marker_margin_w_img(marker_margin_w.size());
00238         vector<PointDouble> marker_margin_b_img(marker_margin_b.size());
00239         H.ProjectPoints(marker_margin_w, marker_margin_w_img);
00240         H.ProjectPoints(marker_margin_b, marker_margin_b_img);
00241         cam->Distort(marker_margin_w_img);
00242         cam->Distort(marker_margin_b_img);
00243 
00244         min = max = 0; // Now min and max values are averages over black and white border pixels.
00245         for (size_t i=0; i<marker_margin_w_img.size(); i++) {
00246                 x = (int)(0.5+Limit(marker_margin_w_img[i].x, 0, gray->width-1));
00247                 y = (int)(0.5+Limit(marker_margin_w_img[i].y, 0, gray->height-1));
00248                 marker_margin_w_img[i].val = (int)cvGetReal2D(gray, y, x);
00249                 max += marker_margin_w_img[i].val;
00250                 //if(marker_margin_w_img[i].val > max) max = marker_margin_w_img[i].val;
00251                 //if(marker_margin_w_img[i].val < min) min = marker_margin_w_img[i].val;
00252         }
00253         for (size_t i=0; i<marker_margin_b_img.size(); i++) {
00254                 x = (int)(0.5+Limit(marker_margin_b_img[i].x, 0, gray->width-1));
00255                 y = (int)(0.5+Limit(marker_margin_b_img[i].y, 0, gray->height-1));
00256                 marker_margin_b_img[i].val = (int)cvGetReal2D(gray, y, x);
00257                 min += marker_margin_b_img[i].val;
00258                 //if(marker_margin_b_img[i].val > max) max = marker_margin_b_img[i].val;
00259                 //if(marker_margin_b_img[i].val < min) min = marker_margin_b_img[i].val;
00260         ros_marker_points_img.push_back(PointDouble(x,y));
00261         }
00262         max /= marker_margin_w_img.size();
00263         min /= marker_margin_b_img.size();
00264 
00265         // Threshold the marker content
00266         cvThreshold(marker_content, marker_content, (max+min)/2.0, 255, CV_THRESH_BINARY);
00267 
00268         // Count erroneous margin nodes
00269         int erroneous = 0;
00270         int total = 0;
00271         for (size_t i=0; i<marker_margin_w_img.size(); i++) {
00272                 if (marker_margin_w_img[i].val < (max+min)/2.0) erroneous++;
00273                 total++;
00274         }
00275         for (size_t i=0; i<marker_margin_b_img.size(); i++) {
00276                 if (marker_margin_b_img[i].val > (max+min)/2.0) erroneous++;
00277                 total++;
00278         }
00279         margin_error = (double)erroneous/total;
00280         track_error;
00281 
00282 #ifdef VISUALIZE_MARKER_POINTS
00283         // Now we fill also this temporary debug table for visualizing marker code reading
00284         // TODO: this whole vector is only for debug purposes
00285         marker_allpoints_img.clear();
00286         for (size_t i=0; i<marker_margin_w_img.size(); i++) {
00287                 PointDouble p = marker_margin_w_img[i];
00288                 if (p.val < (max+min)/2.0) p.val=255; // error
00289                 else p.val=0; // ok
00290                 marker_allpoints_img.push_back(p);
00291         }
00292         for (size_t i=0; i<marker_margin_b_img.size(); i++) {
00293                 PointDouble p = marker_margin_b_img[i];
00294                 if (p.val > (max+min)/2.0) p.val=255; // error
00295                 else p.val=0; // ok
00296                 marker_allpoints_img.push_back(p);
00297         }
00298         for (size_t i=0; i<marker_points_img.size(); i++) {
00299                 PointDouble p = marker_points_img[i];
00300                 p.val=128; // Unknown?
00301                 marker_allpoints_img.push_back(p);
00302         }
00303 #endif
00304         return true;
00305 }
00306 void Marker::UpdatePose(vector<PointDouble > &_marker_corners_img, Camera *cam, int orientation, int frame_no /* =0 */, bool update_pose /* =true */) {
00307         marker_corners_img.resize(_marker_corners_img.size());
00308         copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img.begin());
00309 
00310         // Calculate exterior orientation
00311         if(orientation > 0)
00312                 std::rotate(marker_corners_img.begin(), marker_corners_img.begin() + orientation, marker_corners_img.end());
00313 
00314         if (update_pose) cam->CalcExteriorOrientation(marker_corners, marker_corners_img, &pose);
00315 }
00316 bool Marker::DecodeContent(int *orientation) {
00317         *orientation = 0;
00318         decode_error = 0;
00319         return true;
00320 }
00321 
00322 void Marker::SaveMarkerImage(const char *filename, int save_res) const {
00323         double scale;
00324         if (save_res == 0) {
00325                 // TODO: More intelligent deduction of a minimum save_res
00326                 save_res = int((res+margin+margin)*12);
00327         }
00328         scale = double(save_res)/double(res+margin+margin);
00329 
00330         IplImage *img = cvCreateImage(cvSize(save_res, save_res), IPL_DEPTH_8U, 1);
00331         IplImage *img_content = cvCreateImage(cvSize(int(res*scale+0.5), int(res*scale+0.5)), IPL_DEPTH_8U, 1);
00332         cvZero(img);
00333         CvMat submat;
00334         cvGetSubRect(img, &submat, cvRect(int(margin*scale), int(margin*scale), int(res*scale), int(res*scale)));
00335         cvResize(marker_content, img_content, CV_INTER_NN);
00336         cvCopy(img_content, &submat);
00337         cvSaveImage(filename, img);
00338         cvReleaseImage(&img_content);
00339         cvReleaseImage(&img);
00340 }
00341 
00342 void Marker::ScaleMarkerToImage(IplImage *image) const {
00343         const int multiplier=96;
00344         IplImage *img = cvCreateImage(cvSize(int(multiplier*(res+margin+margin)+0.5), int(multiplier*(res+margin+margin)+0.5)), IPL_DEPTH_8U, 1);
00345         IplImage *img_content = cvCreateImage(cvSize(int(multiplier*res+0.5), int(multiplier*res+0.5)), IPL_DEPTH_8U, 1);
00346         cvZero(img);
00347         CvMat submat;
00348         cvGetSubRect(img, &submat, cvRect(int(multiplier*margin+0.5), int(multiplier*margin+0.5), int(multiplier*res+0.5), int(multiplier*res+0.5)));
00349         cvResize(marker_content, img_content, CV_INTER_NN);
00350         cvCopy(img_content, &submat);
00351         cvResize(img, image, CV_INTER_NN);
00352         cvReleaseImage(&img_content);
00353         cvReleaseImage(&img);
00354 }
00355 
00356 void Marker::SetMarkerSize(double _edge_length, int _res, double _margin) {
00357         // TODO: Is this right place for default marker size?
00358         edge_length = (_edge_length?_edge_length:1);
00359         res = _res; //(_res?_res:10);
00360         margin = (_margin?_margin:1);
00361         double x_min = -0.5*edge_length;
00362         double y_min = -0.5*edge_length;
00363         double x_max = 0.5*edge_length;
00364         double y_max = 0.5*edge_length;
00365         double cx_min = (x_min * res)/(res + margin + margin);
00366         double cy_min = (y_min * res)/(res + margin + margin);
00367         double cx_max = (x_max * res)/(res + margin + margin);
00368         double cy_max = (y_max * res)/(res + margin + margin);
00369         double step = edge_length / (res + margin + margin);
00370 
00371         // marker_corners
00372         marker_corners_img.resize(4);
00373 
00374         // Same order as the detected corners
00375         marker_corners.clear();
00376         marker_corners.push_back(PointDouble(x_min, y_min));
00377         marker_corners.push_back(PointDouble(x_max, y_min));
00378         marker_corners.push_back(PointDouble(x_max, y_max));
00379         marker_corners.push_back(PointDouble(x_min, y_max));
00380 
00381         // Rest can be done only if we have existing resolution
00382         if (res <= 0) return;
00383 
00384         // marker_points
00385         marker_points.clear();
00386         for(int j = 0; j < res; ++j) {
00387                 for(int i = 0; i < res; ++i) {
00388                         PointDouble  pt;
00389                         pt.y = cy_max - (step*j) - (step/2);
00390                         pt.x = cx_min + (step*i) + (step/2);
00391                         marker_points.push_back(pt);
00392                 }
00393         }
00394 
00395         // Samples to be used in margins
00396         // TODO: Now this works only if the "margin" is without decimals
00397         // TODO: This should be made a lot cleaner
00398         marker_margin_w.clear();
00399         marker_margin_b.clear();
00400         for(int j = -1; j<=margin-1; j++) {
00401                 PointDouble  pt;
00402                 // Sides
00403                 for (int i=0; i<res; i++) {
00404                         pt.x = cx_min + step*i + step/2;
00405                         pt.y = y_min + step*j + step/2;
00406                         if (j < 0) marker_margin_w.push_back(pt);
00407                         else marker_margin_b.push_back(pt);
00408                         pt.y = y_max - step*j - step/2;
00409                         if (j < 0) marker_margin_w.push_back(pt);
00410                         else marker_margin_b.push_back(pt);
00411                         pt.y = cy_min + step*i + step/2;
00412                         pt.x = x_min + step*j + step/2;
00413                         if (j < 0) marker_margin_w.push_back(pt);
00414                         else marker_margin_b.push_back(pt);
00415                         pt.x = x_max - step*j - step/2;
00416                         if (j < 0) marker_margin_w.push_back(pt);
00417                         else marker_margin_b.push_back(pt);
00418                 }
00419                 // Corners
00420                 for(int i = -1; i<=margin-1; i++) {
00421                         pt.x = x_min + step*i + step/2;
00422                         pt.y = y_min + step*j + step/2;
00423                         if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
00424                         else marker_margin_b.push_back(pt);
00425                         pt.x = x_min + step*i + step/2;
00426                         pt.y = y_max - step*j - step/2;
00427                         if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
00428                         else marker_margin_b.push_back(pt);
00429                         pt.x = x_max - step*i - step/2;
00430                         pt.y = y_max - step*j - step/2;
00431                         if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
00432                         else marker_margin_b.push_back(pt);
00433                         pt.x = x_max - step*i - step/2;
00434                         pt.y = y_min + step*j + step/2;
00435                         if ((j < 0) || (i < 0)) marker_margin_w.push_back(pt);
00436                         else marker_margin_b.push_back(pt);
00437                 }
00438         }
00439         /*
00440         for(int j = -margin-1; j < res+margin+margin+2; ++j) {
00441                 for(int i = 0; i < res+margin+margin+2; ++i) {
00442                         PointDouble  pt;
00443                         pt.y = y_min - step/2 + step*j;
00444                         pt.x = x_min - step/2 + step*i;
00445                         if ((pt.x < x_min) || (pt.y < y_min) ||
00446                                 (pt.x > x_max) || (pt.y > y_max))
00447                         {
00448                                 marker_margin_w.push_back(pt);
00449                         }
00450                         else 
00451                         if ((pt.x < cx_min) || (pt.y < cy_min) ||
00452                                 (pt.x > cx_max) || (pt.y > cy_max))
00453                         {
00454                                 marker_margin_b.push_back(pt);
00455                         }
00456                 }
00457         }
00458         /*
00459         //double step = edge_length / (res + margin + margin);
00460         for(int j = 0; j < res+margin+margin+2; ++j) {
00461                 for(int i = 0; i < res+margin+margin+2; ++i) {
00462                         PointDouble  pt;
00463                         pt.y = y_min - step/2 + step*j;
00464                         pt.x = x_min - step/2 + step*i;
00465                         if ((pt.x < x_min) || (pt.y < y_min) ||
00466                                 (pt.x > x_max) || (pt.y > y_max))
00467                         {
00468                                 marker_margin_w.push_back(pt);
00469                         }
00470                         else 
00471                         if ((pt.x < cx_min) || (pt.y < cy_min) ||
00472                                 (pt.x > cx_max) || (pt.y > cy_max))
00473                         {
00474                                 marker_margin_b.push_back(pt);
00475                         }
00476                 }
00477         }
00478         */
00479         /*
00480         marker_margin_w.clear();
00481         marker_margin_b.clear();
00482         for (double y=y_min-(step/2); y<y_max+(step/2); y+=step) {
00483                 for (double x=x_min-(step/2); x<x_max+(step/2); x+=step) {
00484                         PointDouble pt(x, y);
00485                         if ((x < x_min) || (y < y_min) ||
00486                                 (x > x_max) || (y > y_max))
00487                         {
00488                                 marker_margin_w.push_back(pt);
00489                         } 
00490                         else 
00491                         if ((x < cx_min) || (y < cy_min) ||
00492                                 (x > cx_max) || (y > cy_max))
00493                         {
00494                                 marker_margin_b.push_back(pt);
00495                         }
00496                 }
00497         }
00498         */
00499         /*
00500         marker_points.clear();
00501         marker_margin_w.clear();
00502         marker_margin_b.clear();
00503         for(int j = 0; j < res+margin+margin+2; ++j) {
00504                 for(int i = 0; i < res+margin+margin+2; ++i) {
00505                         PointDouble  pt;
00506                 }
00507         }
00508         */
00509 
00510         // marker content
00511         if (marker_content) cvReleaseMat(&marker_content);
00512         marker_content = cvCreateMat(res, res, CV_8U);
00513         cvSet(marker_content, cvScalar(255));
00514 }
00515 Marker::~Marker() {
00516         if (marker_content) cvReleaseMat(&marker_content);
00517 }
00518 Marker::Marker(double _edge_length, int _res, double _margin)
00519 {
00520         marker_content = NULL;
00521         margin_error = 0;
00522         decode_error = 0;
00523         track_error = 0;
00524         SetMarkerSize(_edge_length, _res, _margin);
00525         ros_orientation = -1;
00526         ros_corners_3D.resize(4);
00527         valid=false;
00528 }
00529 Marker::Marker(const Marker& m) {
00530         marker_content = NULL;
00531         SetMarkerSize(m.edge_length, m.res, m.margin);
00532 
00533         pose = m.pose;
00534         margin_error = m.margin_error;
00535         decode_error = m.decode_error;
00536         track_error = m.track_error;
00537         cvCopy(m.marker_content, marker_content);
00538     ros_orientation = m.ros_orientation;
00539 
00540         ros_marker_points_img.resize(m.ros_marker_points_img.size());
00541         copy(m.ros_marker_points_img.begin(), m.ros_marker_points_img.end(), ros_marker_points_img.begin());
00542         marker_corners.resize(m.marker_corners.size());
00543         copy(m.marker_corners.begin(), m.marker_corners.end(), marker_corners.begin());
00544         marker_points.resize(m.marker_points.size());
00545         copy(m.marker_points.begin(), m.marker_points.end(), marker_points.begin());
00546         marker_corners_img.resize(m.marker_corners_img.size());
00547         copy(m.marker_corners_img.begin(), m.marker_corners_img.end(), marker_corners_img.begin());
00548     ros_corners_3D.resize(m.ros_corners_3D.size());
00549         copy(m.ros_corners_3D.begin(), m.ros_corners_3D.end(), ros_corners_3D.begin());
00550 
00551         valid = m.valid;
00552 #ifdef VISUALIZE_MARKER_POINTS
00553         marker_allpoints_img.resize(m.marker_allpoints_img.size());
00554         copy(m.marker_allpoints_img.begin(), m.marker_allpoints_img.end(), marker_allpoints_img.begin());
00555 #endif
00556 }
00557 
00558 bool MarkerArtoolkit::DecodeContent(int *orientation) {
00559         int a = (int)cvGetReal2D(marker_content, 0, 0);
00560         int b = (int)cvGetReal2D(marker_content, res-1, 0);
00561         int c = (int)cvGetReal2D(marker_content, res-1, res-1);
00562         int d = (int)cvGetReal2D(marker_content, 0, res-1);
00563         if (!a && !b && c) *orientation = 0;
00564         else if (!b && !c && d) *orientation = 1;
00565         else if (!c && !d && a) *orientation = 2;
00566         else if (!d && !a && b) *orientation = 3;
00567         else return false;
00568 
00569         Bitset bs;
00570         bs.clear();
00571         for (int j = 0; j < res; j++) {
00572                 for (int i = 0; i < res ; i++) {
00573                         if (*orientation == 0) {
00574                                 if ((j == 0)     && (i == 0)) continue;
00575                                 if ((j == res-1) && (i == 0)) continue;
00576                                 if ((j == res-1) && (i == res-1)) continue;
00577                                 if (cvGetReal2D(marker_content, j, i)) bs.push_back(false);
00578                                 else bs.push_back(true);
00579                         }
00580                         else if (*orientation == 1) {
00581                                 if (((res-i-1) == res-1) && (j == 0)) continue;
00582                                 if (((res-i-1) == res-1) && (j == res-1)) continue;
00583                                 if (((res-i-1) == 0)     && (j == res-1)) continue;
00584                                 if (cvGetReal2D(marker_content, res-i-1, j)) bs.push_back(false);
00585                                 else bs.push_back(true);
00586                         }
00587                         else if (*orientation == 2) {
00588                                 if (((res-j-1) == res-1) && ((res-i-1) == res-1)) continue;
00589                                 if (((res-j-1) == 0)     && ((res-i-1) == res-1)) continue;
00590                                 if (((res-j-1) == 0)     && ((res-i-1) == 0)) continue;
00591                                 if (cvGetReal2D(marker_content, res-j-1, res-i-1)) bs.push_back(false);
00592                                 else bs.push_back(true);
00593                         }
00594                         else if (*orientation == 3) {
00595                                 if ((i == 0)     && ((res-j-1) == res-1)) continue;
00596                                 if ((i == 0)     && ((res-j-1) == 0)) continue;
00597                                 if ((i == res-1) && ((res-j-1) == 0)) continue;
00598                                 if (cvGetReal2D(marker_content, i, res-j-1)) bs.push_back(false);
00599                                 else bs.push_back(true);
00600                         }
00601                 }
00602         }
00603         id = bs.ulong();
00604         return true;
00605 }
00606 
00607 void MarkerArtoolkit::SetContent(unsigned long _id) {
00608         // Fill in the content values
00609         margin_error = 0;
00610         decode_error = 0;
00611         id = _id;
00612         Bitset bs;
00613         bs.push_back_meaningful(_id);
00614         for (int j = res-1; j >= 0; j--) {
00615                 for (int i = res-1; i >= 0 ; i--) {
00616                         if ((j == 0)     && (i == 0))
00617                                 cvSetReal2D(marker_content, j, i, 0);
00618                         else if ((j == res-1) && (i == 0))
00619                                 cvSetReal2D(marker_content, j, i, 0);
00620                         else if ((j == res-1) && (i == res-1)) 
00621                                 cvSetReal2D(marker_content, j, i, 255);
00622                         else {
00623                                 if (bs.Length() && bs.pop_back())
00624                                         cvSetReal2D(marker_content, j, i, 0);
00625                                 else
00626                                         cvSetReal2D(marker_content, j, i, 255);
00627                         }
00628                 }
00629         }
00630 }
00631 
00632 void MarkerData::DecodeOrientation(int *error, int *total, int *orientation) {
00633         int i,j;
00634         vector<double> errors(4);
00635         int color = 255;
00636 
00637         // Resolution identification
00638         j = res/2;
00639         for (i=0; i<res; i++) {
00640                 (*total)++;
00641                 if ((int)cvGetReal2D(marker_content, j, i) != color) errors[0]++;
00642                 if ((int)cvGetReal2D(marker_content, i, j) != color) errors[1]++;
00643                 color = (color?0:255);
00644         }
00645         errors[2] = errors[0];
00646         errors[3] = errors[1];
00647 
00648         // Orientation identification
00649         i = res/2;
00650         for (j = (res/2)-2; j <= (res/2)+2; j++) {
00651                 if (j < (res/2)) {
00652                         (*total)++;
00653                         if ((int)cvGetReal2D(marker_content, j, i)       !=   0) errors[0]++;
00654                         if ((int)cvGetReal2D(marker_content, i, j)       !=   0) errors[1]++;
00655                         if ((int)cvGetReal2D(marker_content, j, i)       != 255) errors[2]++;
00656                         if ((int)cvGetReal2D(marker_content, i, j)       != 255) errors[3]++;
00657                 } else if (j > (res/2)) {
00658                         (*total)++;
00659                         if ((int)cvGetReal2D(marker_content, j, i)       != 255) errors[0]++;
00660                         if ((int)cvGetReal2D(marker_content, i, j)       != 255) errors[1]++;
00661                         if ((int)cvGetReal2D(marker_content, j, i)       !=   0) errors[2]++;
00662                         if ((int)cvGetReal2D(marker_content, i, j)       !=   0) errors[3]++;
00663                 }
00664         }
00665         *orientation = min_element(errors.begin(), errors.end()) - errors.begin();
00666         *error = int(errors[*orientation]);
00667         //*orientation = 0; // ttehop
00668 }
00669 
00670 bool MarkerData::DetectResolution(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam) {
00671         vector<PointDouble> marker_corners_img_undist;
00672         marker_corners_img_undist.resize(_marker_corners_img.size());
00673         copy(_marker_corners_img.begin(), _marker_corners_img.end(), marker_corners_img_undist.begin());
00674 
00675         // line_points
00676         std::vector<PointDouble> line_points;
00677         PointDouble pt;
00678         line_points.clear();
00679         pt.x=0; pt.y=0; line_points.push_back(pt);
00680         pt.x=-0.5*edge_length; pt.y=0; line_points.push_back(pt);
00681         pt.x=+0.5*edge_length; pt.y=0; line_points.push_back(pt);
00682         pt.x=0; pt.y=-0.5*edge_length; line_points.push_back(pt);
00683         pt.x=0; pt.y=+0.5*edge_length; line_points.push_back(pt);
00684 
00685         // Figure out the marker point position in the image
00686         // TODO: Note that line iterator cannot iterate outside image
00687         //       therefore we need to distort the endpoints and iterate straight lines.
00688         //       Right way would be to iterate undistorted lines and distort line points.
00689         Homography H;
00690         vector<PointDouble> line_points_img(line_points.size());
00691         line_points_img.resize(line_points.size());
00692         cam->Undistort(marker_corners_img_undist);
00693         H.Find(marker_corners, marker_corners_img_undist);
00694         H.ProjectPoints(line_points, line_points_img);
00695         cam->Distort(line_points_img);
00696 
00697         // Now we have undistorted line end points
00698         // Find lines and then distort the whole line
00699         int white_count[4] = {0}; // white counts for lines 1->0, 2->0, 3->0, 4->0
00700         CvPoint pt1, pt2;
00701         pt2.x = int(line_points_img[0].x);
00702         pt2.y = int(line_points_img[0].y);
00703         if ((pt2.x < 0) || (pt2.y < 0) ||
00704                 (pt2.x >= gray->width) || (pt2.y >= gray->height))
00705         {
00706                 return false;
00707         }
00708         bool white=true;
00709         for (int i=0; i<4; i++) {
00710                 CvLineIterator iterator;
00711                 pt1.x = int(line_points_img[i+1].x);
00712                 pt1.y = int(line_points_img[i+1].y);
00713                 if ((pt1.x < 0) || (pt1.y < 0) ||
00714                         (pt1.x >= gray->width) || (pt1.y >= gray->height))
00715                 {
00716                         return false;
00717                 }
00718                 int count = cvInitLineIterator(gray, pt1, pt2, &iterator, 8, 0);
00719                 std::vector<uchar> vals;
00720                 for(int ii = 0; ii < count; ii++ ){
00721                         CV_NEXT_LINE_POINT(iterator);
00722                         vals.push_back(*(iterator.ptr));
00723                 }
00724                 uchar vmin = *(std::min_element(vals.begin(), vals.end()));
00725                 uchar vmax = *(std::max_element(vals.begin(), vals.end()));
00726                 uchar thresh = (vmin+vmax)/2;
00727                 white=true;
00728                 int bc=0, wc=0, N=2;
00729                 for (size_t ii=0; ii<vals.size(); ii++) {
00730                         // change the color status if we had 
00731                         // N subsequent pixels of the other color
00732                         if (vals[ii] < thresh) { bc++; wc=0; }
00733                         else                   { wc++; bc=0; }
00734                         
00735                         if (white && (bc >= N)) {
00736                                 white=false;
00737                         } 
00738                         else if (!white && (wc >= N)) {
00739                                 white=true;
00740                                 white_count[i]++;
00741                         }
00742                 }
00743         }
00744 
00745         if ((white_count[0]+white_count[1]) == (white_count[2]+white_count[3])) return false;
00746         else if ((white_count[0]+white_count[1]) > (white_count[2]+white_count[3])) {
00747                 if (white_count[0] != white_count[1]) return false;
00748                 if (white_count[0] < 2) return false;
00749                 int nof_whites = white_count[0]*2-(white?1:0); // 'white' contains middle color
00750                 int new_res = 2*nof_whites-1;
00751                 SetMarkerSize(edge_length, new_res, margin);
00752         } 
00753         else {
00754                 if (white_count[2] != white_count[3]) return false;
00755                 if (white_count[2] < 2) return false;
00756                 if (((white_count[2]%2) == 0) != white) return false;
00757                 int nof_whites = white_count[2]*2-(white?1:0);
00758                 int new_res = 2*nof_whites-1;
00759                 SetMarkerSize(edge_length, new_res, margin);
00760         }
00761         return true;
00762 }
00763 
00764 bool MarkerData::UpdateContent(vector<PointDouble > &_marker_corners_img, IplImage *gray, Camera *cam, int frame_no /*= 0*/) {
00765         if (res == 0) {
00766                 if (!DetectResolution(_marker_corners_img, gray, cam)) return false;
00767         }
00768         return UpdateContentBasic(_marker_corners_img, gray, cam, frame_no);
00769 }
00770 
00771 int MarkerData::DecodeCode(int orientation, BitsetExt *bs, int *erroneous, int *total, 
00772         unsigned char* content_type) 
00773 {
00774         // TODO: The orientation isn't fully understood?
00775         //for (int j = res-1; j >= 0; j--) {
00776         for (int j = 0; j < res; j++) {
00777                 for (int i = 0; i < res ; i++) {
00778                         // TODO: Does this work ok for larger markers?
00779                         if ((orientation == 0) || (orientation == 2)) {
00780                                 if (j == res/2) continue;
00781                                 if ((i == res/2) && (j >= (res/2)-2) && (j <= (res/2)+2)) continue;
00782                         } else {
00783                                 if (i == res/2) continue;
00784                                 if ((j == res/2) && (i >= (res/2)-2) && (i <= (res/2)+2)) continue;
00785                         }
00786                         int color = 0;
00787                         if (orientation == 0) color = (int)cvGetReal2D(marker_content, j, i);
00788                         else if (orientation == 1) color = (int)cvGetReal2D(marker_content, res-i-1, j);
00789                         else if (orientation == 2) color = (int)cvGetReal2D(marker_content, res-j-1, res-i-1);
00790                         else if (orientation == 3) color = (int)cvGetReal2D(marker_content, i, res-j-1);
00791                         if (color) bs->push_back(false);
00792                         else bs->push_back(true);
00793                         (*total)++;
00794                 }
00795         }
00796 
00797         unsigned char flags = 0;
00798         int errors = 0;
00799 
00800         // if we have larger than 16-bit code, then we have a header; 16-bit code has a
00801         // hamming(8,4) coded number
00802         if(bs->Length() > 16){
00803                 // read header (8-bit hamming(8,4) -> 4-bit flags)
00804                 BitsetExt header;
00805 
00806                 for(int i = 0; i < HEADER_SIZE; i++)
00807                         header.push_back(bs->pop_front());
00808                         
00809                 errors = header.hamming_dec(8);
00810                 if(errors == -1){
00811                         //OutputDebugString("header decoding failed!!!!!\n");
00812                         return errors;
00813                 }
00814 
00815                 flags = header.uchar();
00816         }
00817         else
00818                 flags &= MARKER_CONTENT_TYPE_NUMBER;
00819 
00820         // check which hamming we are using
00821         //bs->Output(cout); cout<<endl;
00822         if(flags & 0x8) errors = bs->hamming_dec(16);
00823         else errors = bs->hamming_dec(8);
00824         *content_type = flags & 0x7;
00825         
00826         if (errors > 0) (*erroneous) += errors;
00827         return errors;  
00828 }
00829 void MarkerData::Read6bitStr(BitsetExt *bs, char *s, size_t s_max_len) {
00830         deque<bool> bits = bs->GetBits();
00831         deque<bool>::const_iterator iter;
00832         size_t len = 0;
00833         int bitpos = 5;
00834         unsigned long c=0;
00835         for (iter = bits.begin(); iter != bits.end(); iter++) {
00836                 if (*iter) c |= (0x01 << bitpos);
00837                 bitpos--;
00838                 if (bitpos < 0) {
00839                         if (c == 000)                      s[len] = ':';
00840                         else if ((c >= 001) && (c <= 032)) s[len] = 'a' + (char)c - 1;
00841                         else if ((c >= 033) && (c <= 044)) s[len] = '0' + (char)c - 1;
00842                         else if (c == 045)                 s[len] = '+';
00843                         else if (c == 046)                 s[len] = '-';
00844                         else if (c == 047)                 s[len] = '*';
00845                         else if (c == 050)                 s[len] = '/';
00846                         else if (c == 051)                 s[len] = '(';
00847                         else if (c == 052)                 s[len] = ')';
00848                         else if (c == 053)                 s[len] = '$';
00849                         else if (c == 054)                 s[len] = '=';
00850                         else if (c == 055)                 s[len] = ' ';
00851                         else if (c == 056)                 s[len] = ',';
00852                         else if (c == 057)                 s[len] = '.';
00853                         else if (c == 060)                 s[len] = '#';
00854                         else if (c == 061)                 s[len] = '[';
00855                         else if (c == 062)                 s[len] = ']';
00856                         else if (c == 063)                 s[len] = '%';
00857                         else if (c == 064)                 s[len] = '\"';
00858                         else if (c == 065)                 s[len] = '_';
00859                         else if (c == 066)                 s[len] = '!';
00860                         else if (c == 067)                 s[len] = '&';
00861                         else if (c == 070)                 s[len] = '\'';
00862                         else if (c == 071)                 s[len] = '?';
00863                         else if (c == 072)                 s[len] = '<';
00864                         else if (c == 073)                 s[len] = '>';
00865                         else if (c == 074)                 s[len] = '@';
00866                         else if (c == 075)                 s[len] = '\\';
00867                         else if (c == 076)                 s[len] = '^';
00868                         else if (c == 077)                 s[len] = ';';
00869                         else s[len] = '?';
00870                         len++; 
00871                         if (len >= (s_max_len-1)) break;
00872                         bitpos=5; c=0;
00873                 }
00874         }
00875         s[len] = 0;
00876 }
00877 
00878 bool MarkerData::DecodeContent(int *orientation) {
00879         //bool decode(vector<int>& colors, int *orientation, double *error) {
00880         *orientation = 0;
00881 
00882         BitsetExt bs;
00883         int erroneous=0;
00884         int total=0;
00885 
00886         DecodeOrientation(&erroneous, &total, orientation);
00887         int err = DecodeCode(*orientation, &bs, &erroneous, &total, &content_type);
00888         if(err == -1) {
00889                 // couldn't fix 
00890                 decode_error = DBL_MAX;
00891                 return false;
00892         }
00893 
00894         if(content_type == MARKER_CONTENT_TYPE_NUMBER){
00895                 data.id = bs.ulong();
00896         } 
00897         else {
00898                 Read6bitStr(&bs, data.str, MAX_MARKER_STRING_LEN);
00899         }
00900 
00901         decode_error = (double)(erroneous)/total;
00902         
00903         return true;
00904 }
00905 
00906 void MarkerData::Add6bitStr(BitsetExt *bs, char *s) {
00907         while (*s) {
00908                 unsigned char c = (unsigned char)*s;
00909                 if (c == ':')                      bs->push_back((unsigned char)0,6);
00910                 else if ((c >= 'A') && (c <= 'Z')) bs->push_back((unsigned char)(001 + c - 'A'),6);
00911                 else if ((c >= 'a') && (c <= 'z')) bs->push_back((unsigned char)(001 + c - 'a'),6);
00912                 else if ((c >= '0') && (c <= '9')) bs->push_back((unsigned char)(033 + c - '0'),6);
00913                 else if (c == '+')                 bs->push_back((unsigned char)045,6);
00914                 else if (c == '-')                 bs->push_back((unsigned char)046,6);
00915                 else if (c == '*')                 bs->push_back((unsigned char)047,6);
00916                 else if (c == '/')                 bs->push_back((unsigned char)050,6);
00917                 else if (c == '(')                 bs->push_back((unsigned char)051,6);
00918                 else if (c == ')')                 bs->push_back((unsigned char)052,6);
00919                 else if (c == '$')                 bs->push_back((unsigned char)053,6);
00920                 else if (c == '=')                 bs->push_back((unsigned char)054,6);
00921                 else if (c == ' ')                 bs->push_back((unsigned char)055,6);
00922                 else if (c == ',')                 bs->push_back((unsigned char)056,6);
00923                 else if (c == '.')                 bs->push_back((unsigned char)057,6);
00924                 else if (c == '#')                 bs->push_back((unsigned char)060,6);
00925                 else if (c == '[')                 bs->push_back((unsigned char)061,6);
00926                 else if (c == ']')                 bs->push_back((unsigned char)062,6);
00927                 else if (c == '%')                 bs->push_back((unsigned char)063,6);
00928                 else if (c == '\"')                bs->push_back((unsigned char)064,6);
00929                 else if (c == '_')                 bs->push_back((unsigned char)065,6);
00930                 else if (c == '!')                 bs->push_back((unsigned char)066,6);
00931                 else if (c == '&')                 bs->push_back((unsigned char)067,6);
00932                 else if (c == '\'')                bs->push_back((unsigned char)070,6);
00933                 else if (c == '?')                 bs->push_back((unsigned char)071,6);
00934                 else if (c == '<')                 bs->push_back((unsigned char)072,6);
00935                 else if (c == '>')                 bs->push_back((unsigned char)073,6);
00936                 else if (c == '@')                 bs->push_back((unsigned char)074,6);
00937                 else if (c == '\\')                bs->push_back((unsigned char)075,6);
00938                 else if (c == '^')                 bs->push_back((unsigned char)076,6);
00939                 else if (c == ';')                 bs->push_back((unsigned char)077,6);
00940                 else                               bs->push_back((unsigned char)071,6);
00941                 s++;
00942         }
00943 }
00944 
00945 int MarkerData::UsableDataBits(int marker_res, int hamming) {
00946         if (marker_res < 5) return 0;
00947         if (!(marker_res % 2)) return 0;
00948         int bits = marker_res * marker_res;
00949         if (marker_res > 5) bits -= 8; // With larger resolutions we reserve 8 bits for hamming(8,4) encoded 4 flags
00950         bits -= marker_res;            // center line indicating the resolution
00951         bits -= 4;                     // the four pixels indicating the orientation
00952         int tail = bits % hamming; 
00953         if (tail < 3) bits -= tail;    // hamming can't use tail pixels if there is only 2 or 1 of them
00954         return bits;
00955 }
00956 
00957 void MarkerData::SetContent(MarkerContentType _content_type, unsigned long _id, const char *_str, bool force_strong_hamming, bool verbose) {
00958         // Fill in the content values
00959         content_type = _content_type;
00960         margin_error = 0;
00961         decode_error = 0;
00962         if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
00963                 data.id = _id;
00964         } else {
00965                 STRCPY(data.str, MAX_MARKER_STRING_LEN, _str);
00966         }
00967         // Encode
00968         const int max_marker_res = 127;
00969         BitsetExt bs_flags(verbose);
00970         BitsetExt bs_data(verbose);
00971         int enc_bits;  // How many encoded bits fits in the marker
00972         int data_bits; // How many data bits fit inside the encoded bits
00973         int hamming;   // Do we use 8-bit or 16-bit hamming?
00974         if (content_type == MARKER_CONTENT_TYPE_NUMBER) {
00975                 bs_data.push_back_meaningful(data.id);
00976                 for (res=5; res<max_marker_res; res+=2) {
00977                         hamming = 8;
00978                         enc_bits = UsableDataBits(res, hamming);
00979                         data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
00980                         if (data_bits >= bs_data.Length()) break;
00981                         if ((res > 5) && !force_strong_hamming) {
00982                                 hamming = 16;
00983                                 enc_bits = UsableDataBits(res, hamming);
00984                                 data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
00985                                 if (data_bits >= bs_data.Length()) break;
00986                         }
00987                 }
00988                 bs_data.fill_zeros_left(data_bits);
00989                 bs_data.hamming_enc(hamming);
00990                 if (verbose) {
00991                         cout<<"Using hamming("<<hamming<<") for "<<res<<"x"<<res<<" marker"<<endl;
00992                         cout<<bs_data.Length()<<" bits are filled into "<<data_bits;
00993                         cout<<" bits, and encoded into "<<enc_bits<<" bits"<<endl;
00994                         cout<<"data src: "; bs_data.Output(cout); cout<<endl;
00995                         cout<<"data enc: "; bs_data.Output(cout); cout<<endl;
00996                 }
00997                 if (res > 5) {
00998                         if (hamming == 16) bs_flags.push_back(true);
00999                         else bs_flags.push_back(false);
01000                         bs_flags.push_back((unsigned long)0,3);
01001                         bs_flags.hamming_enc(8);
01002                         if (verbose) {
01003                                 cout<<"flags src: "; bs_flags.Output(cout); cout<<endl;
01004                                 cout<<"flags enc: "; bs_flags.Output(cout); cout<<endl;
01005                         }
01006                 }
01007         } else {
01008                 Add6bitStr(&bs_data, data.str);
01009                 for (res=7; res<max_marker_res; res+=2) {
01010                         hamming = 8;
01011                         enc_bits = UsableDataBits(res, hamming);
01012                         data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
01013                         if (data_bits >= bs_data.Length()) break;
01014                         if (!force_strong_hamming) {
01015                                 hamming = 16;
01016                                 enc_bits = UsableDataBits(res, hamming);
01017                                 data_bits = BitsetExt::count_hamming_dec_len(hamming, enc_bits);
01018                                 if (data_bits >= bs_data.Length()) break;
01019                         }
01020                 }
01021                 while (bs_data.Length() < ((data_bits/6)*6)) {
01022                         bs_data.push_back((unsigned char)055,6); // Add space
01023                 }
01024                 while (bs_data.Length() < data_bits) {
01025                         bs_data.push_back(false); // Add 0
01026                 }
01027                 bs_data.hamming_enc(hamming);
01028                 if (hamming == 16) bs_flags.push_back(true);
01029                 else bs_flags.push_back(false);
01030                 if (content_type == MARKER_CONTENT_TYPE_STRING) bs_flags.push_back((unsigned long)1,3);
01031                 else if (content_type == MARKER_CONTENT_TYPE_FILE) bs_flags.push_back((unsigned long)2,3);
01032                 else if (content_type == MARKER_CONTENT_TYPE_HTTP) bs_flags.push_back((unsigned long)3,3);
01033                 bs_flags.hamming_enc(8);
01034                 if (verbose) {
01035                         cout<<"Using hamming("<<hamming<<") for "<<res<<"x"<<res<<" marker"<<endl;
01036                         cout<<bs_data.Length()<<" bits are filled into "<<data_bits;
01037                         cout<<" bits, and encoded into "<<enc_bits<<" bits"<<endl;
01038                         cout<<"data src: "; bs_data.Output(cout); cout<<endl;
01039                         cout<<"data enc: "; bs_data.Output(cout); cout<<endl;
01040                         cout<<"flags src: "; bs_flags.Output(cout); cout<<endl;
01041                         cout<<"flags enc: "; bs_flags.Output(cout); cout<<endl;
01042                 }
01043         }
01044         
01045         // Fill in the marker content
01046         deque<bool> bs(bs_flags.GetBits());
01047         bs.insert(bs.end(), bs_data.GetBits().begin(), bs_data.GetBits().end());
01048         deque<bool>::const_iterator iter = bs.begin();
01049         SetMarkerSize(edge_length, res, margin);
01050         cvSet(marker_content, cvScalar(255));
01051         for (int j=0; j<res; j++) {
01052                 for (int i=0; i<res; i++) {
01053                         if (j == res/2) {
01054                                 if (i%2) cvSet2D(marker_content, j, i, cvScalar(0));
01055                         } else if ((i == res/2) && (j < res/2) && (j >= (res/2)-2)) {
01056                                 cvSet2D(marker_content, j, i, cvScalar(0));
01057                         } else if ((i == res/2) && (j > res/2) && (j <= (res/2)+2)) {
01058                                 cvSet2D(marker_content, j, i, cvScalar(255));
01059                         } else {
01060                                 if (iter != bs.end()) {
01061                                         if (*iter) cvSet2D(marker_content, j, i, cvScalar(0));
01062                                         iter++;
01063                                 }
01064                         }
01065                 }
01066         }
01067 }
01068 
01069 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sun Oct 5 2014 22:16:26