MarkerDetector.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 "ar_track_alvar/MarkerDetector.h"
00025 
00026 template class ALVAR_EXPORT alvar::MarkerDetector<alvar::Marker>;
00027 template class ALVAR_EXPORT alvar::MarkerDetector<alvar::MarkerData>;
00028 template class ALVAR_EXPORT alvar::MarkerDetector<alvar::MarkerArtoolkit>;
00029 
00030 using namespace std;
00031 
00032 namespace alvar {
00033         MarkerDetectorImpl::MarkerDetectorImpl() {
00034                 SetMarkerSize();
00035                 SetOptions();
00036                 labeling = NULL;
00037         }
00038 
00039         MarkerDetectorImpl::~MarkerDetectorImpl() {
00040                 if (labeling) delete labeling;
00041         }
00042 
00043         void MarkerDetectorImpl::TrackMarkersReset() {
00044                 _track_markers_clear();
00045         }
00046 
00047         void MarkerDetectorImpl::TrackMarkerAdd(int id, PointDouble corners[4]) {
00048     Marker *mn = new_M(edge_length, res, margin);
00049                 if (map_edge_length.find(id) != map_edge_length.end()) {
00050                         mn->SetMarkerSize(map_edge_length[id], res, margin);
00051                 }
00052 
00053                 mn->SetId(id);
00054                 mn->marker_corners_img.clear();
00055                 mn->marker_corners_img.push_back(corners[0]);
00056                 mn->marker_corners_img.push_back(corners[1]);
00057                 mn->marker_corners_img.push_back(corners[2]);
00058                 mn->marker_corners_img.push_back(corners[3]);
00059                 _track_markers_push_back(mn);
00060     delete mn;
00061         }
00062 
00063         void MarkerDetectorImpl::SetMarkerSize(double _edge_length, int _res, double _margin) {
00064                 edge_length = _edge_length;
00065                 res = _res;
00066                 margin = _margin;
00067                 map_edge_length.clear(); // TODO: Should we clear these here?
00068   }
00069 
00070         void MarkerDetectorImpl::SetMarkerSizeForId(unsigned long id, double _edge_length) {
00071                 map_edge_length[id] = _edge_length;
00072         }
00073 
00074         void MarkerDetectorImpl::SetOptions(bool _detect_pose_grayscale) {
00075                 detect_pose_grayscale = _detect_pose_grayscale;
00076         }
00077 
00078         int MarkerDetectorImpl::Detect(IplImage *image,
00079                            Camera *cam,
00080                            bool track,
00081                            bool visualize,
00082                            double max_new_marker_error,
00083                            double max_track_error,
00084                            LabelingMethod labeling_method,
00085                            bool update_pose)
00086         {
00087                 assert(image->origin == 0); // Currently only top-left origin supported
00088                 double error=-1;
00089 
00090                 // Swap marker tables
00091                 _swap_marker_tables();
00092                 _markers_clear();
00093 
00094                 switch(labeling_method)
00095                 {
00096                         case CVSEQ :
00097                 
00098                                 if(!labeling)
00099                                         labeling = new LabelingCvSeq();
00100                                 ((LabelingCvSeq*)labeling)->SetOptions(detect_pose_grayscale);
00101                                 break;
00102                 }
00103 
00104                 labeling->SetCamera(cam);
00105                 labeling->LabelSquares(image, visualize);
00106                 vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
00107                 IplImage* gray = labeling->gray;
00108 
00109                 int orientation;
00110 
00111                 // When tracking we find the best matching blob and test if it is near enough?
00112                 if (track) {
00113                         for (size_t ii=0; ii<_track_markers_size(); ii++) {
00114                                 Marker *mn = _track_markers_at(ii);
00115                                 if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
00116                                 int track_i=-1;
00117                                 int track_orientation=0;
00118                                 double track_error=1e200;
00119                                 for(unsigned i = 0; i < blob_corners.size()/*blobs_ret.size()*/; ++i) {
00120                                         if (blob_corners[i].empty()) continue;
00121                                         mn->CompareCorners(blob_corners[i], &orientation, &error);
00122                                         if (error < track_error) {
00123                                                 track_i = i;
00124                                                 track_orientation = orientation;
00125                                                 track_error = error;
00126                                         }
00127                                 }
00128                                 if (track_error <= max_track_error) {
00129                                         mn->SetError(Marker::DECODE_ERROR, 0);
00130                                         mn->SetError(Marker::MARGIN_ERROR, 0);
00131                                         mn->SetError(Marker::TRACK_ERROR, track_error);
00132                     mn->UpdateContent(blob_corners[track_i], gray, cam);    //Maybe should only do this when kinect is being used? Don't think it hurts anything...
00133                                         mn->UpdatePose(blob_corners[track_i], cam, track_orientation, update_pose);
00134                                         _markers_push_back(mn);
00135                                         blob_corners[track_i].clear(); // We don't want to handle this again...
00136                                         if (visualize) mn->Visualize(image, cam, CV_RGB(255,255,0));
00137                                 }
00138                         }
00139                 }
00140 
00141                 // Now we go through the rest of the blobs -- in case there are new markers...
00142                 for(size_t i = 0; i < blob_corners.size(); ++i)
00143                 {
00144                         if (blob_corners[i].empty()) continue;
00145 
00146                         Marker *mn = new_M(edge_length, res, margin);
00147                         bool ub = mn->UpdateContent(blob_corners[i], gray, cam);
00148             bool db = mn->DecodeContent(&orientation); 
00149                         if (ub && db &&
00150                                 (mn->GetError(Marker::MARGIN_ERROR | Marker::DECODE_ERROR) <= max_new_marker_error))
00151                         {
00152                                 if (map_edge_length.find(mn->GetId()) != map_edge_length.end()) {
00153                                         mn->SetMarkerSize(map_edge_length[mn->GetId()], res, margin);
00154                                 }
00155                                 mn->UpdatePose(blob_corners[i], cam, orientation, update_pose);
00156                 mn->ros_orientation = orientation;
00157                                 _markers_push_back(mn);
00158  
00159                                 if (visualize) mn->Visualize(image, cam, CV_RGB(255,0,0));
00160                         }
00161 
00162                         delete mn;
00163                 }
00164 
00165                 return (int) _markers_size();
00166         }
00167 
00168         int MarkerDetectorImpl::DetectAdditional(IplImage *image, Camera *cam, bool visualize, double max_track_error)
00169         {
00170                 assert(image->origin == 0); // Currently only top-left origin supported
00171                 if(!labeling) return -1;
00172                 double error=-1;
00173                 int orientation;
00174                 int count=0;
00175                 vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
00176 
00177                 for (size_t ii=0; ii<_track_markers_size(); ii++) {
00178                         Marker *mn = _track_markers_at(ii);
00179                         if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
00180                         int track_i=-1;
00181                         int track_orientation=0;
00182                         double track_error=1e200;
00183                         for(unsigned i = 0; i < blob_corners.size(); ++i) {
00184                                 if (blob_corners[i].empty()) continue;
00185                                 mn->CompareCorners(blob_corners[i], &orientation, &error);
00186                                 if (error < track_error) {
00187                                         track_i = i;
00188                                         track_orientation = orientation;
00189                                         track_error = error;
00190                                 }
00191                         }
00192                         if (track_error <= max_track_error) {
00193                                 mn->SetError(Marker::DECODE_ERROR, 0);
00194                                 mn->SetError(Marker::MARGIN_ERROR, 0);
00195                                 mn->SetError(Marker::TRACK_ERROR, track_error);
00196                                 mn->UpdatePose(blob_corners[track_i], cam, track_orientation);
00197                                 _markers_push_back(mn);
00198                                 count++;
00199                                 blob_corners[track_i].clear(); // We don't want to handle this again...
00200 
00201                                 if (visualize) {
00202                                         mn->Visualize(image, cam, CV_RGB(0,255,255));
00203                                 }
00204                         }
00205                 }
00206                 return count;
00207         }
00208 
00209 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02