MultiMarker.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/Alvar.h"
00025 #include "ar_track_alvar/MultiMarker.h"
00026 #include "ar_track_alvar/FileFormatUtils.h"
00027 #include <fstream>
00028 
00029 using namespace std;
00030 
00031 namespace alvar {
00032 using namespace std;
00033 
00034 int MultiMarker::pointcloud_index(int marker_id, int marker_corner, bool add_if_missing /*=false*/) {
00035         return (get_id_index(marker_id ,add_if_missing)*4)+marker_corner;
00036 }
00037 
00038 int MultiMarker::get_id_index(int id, bool add_if_missing /*=false*/)
00039 {
00040         for(size_t i = 0; i < marker_indices.size(); ++i) {
00041                 if(marker_indices.at(i) == id)
00042                         return (int) i;
00043         }
00044         if (!add_if_missing) return -1;
00045         marker_indices.push_back(id);
00046         marker_status.push_back(0);
00047         return (marker_indices.size()-1);
00048 }
00049 
00050 void MultiMarker::Reset()
00051 {
00052         fill(marker_status.begin(), marker_status.end(), 0);
00053         pointcloud.clear();
00054 }
00055 
00056 bool MultiMarker::SaveXML(const char* fname) {
00057         TiXmlDocument document;
00058         document.LinkEndChild(new TiXmlDeclaration("1.0", "UTF-8", "no"));
00059         document.LinkEndChild(new TiXmlElement("multimarker"));
00060         TiXmlElement *xml_root = document.RootElement();
00061 
00062         int n_markers = marker_indices.size();
00063         xml_root->SetAttribute("markers", n_markers);
00064 
00065         for(int i = 0; i < n_markers; ++i) {
00066                 TiXmlElement *xml_marker = new TiXmlElement("marker");
00067                 xml_root->LinkEndChild(xml_marker);
00068 
00069                 xml_marker->SetAttribute("index", marker_indices[i]);
00070                 xml_marker->SetAttribute("status", marker_status[i]);
00071 
00072                 for(int j = 0; j < 4; ++j) {
00073                         TiXmlElement *xml_corner = new TiXmlElement("corner");
00074                         xml_marker->LinkEndChild(xml_corner);
00075                         CvPoint3D64f X = pointcloud[pointcloud_index(marker_indices[i], j)];
00076                         xml_corner->SetDoubleAttribute("x", X.x);
00077                         xml_corner->SetDoubleAttribute("y", X.y);
00078                         xml_corner->SetDoubleAttribute("z", X.z);
00079                 }
00080         }
00081         return document.SaveFile(fname);
00082 }
00083 
00084 bool MultiMarker::SaveText(const char* fname) {
00085         size_t n_markers = marker_indices.size();
00086 
00087         fstream file_op(fname, ios::out);
00088 
00089         file_op<<n_markers<<endl;
00090 
00091         file_op<<endl;
00092 
00093         for(size_t i = 0; i < n_markers; ++i)
00094                 file_op<<marker_indices[i]<<endl;
00095 
00096         file_op<<endl;
00097 
00098         for(size_t i = 0; i < n_markers; ++i)
00099                 file_op<<marker_status[i]<<endl;
00100 
00101         file_op<<endl;
00102 
00103         for(size_t i = 0; i < n_markers; ++i)
00104                 for(size_t j = 0; j < 4; ++j)
00105                 {
00106                         CvPoint3D64f X = pointcloud[pointcloud_index(marker_indices[i], j)];
00107                         file_op<<X.x<<" "<<X.y<<" "<<X.z<<endl;
00108                         
00109                 }
00110         file_op.close();
00111 
00112         return true;
00113 }
00114 
00115 bool MultiMarker::Save(const char* fname, FILE_FORMAT format) {
00116         switch (format) {
00117                 case FILE_FORMAT_XML:
00118                         return SaveXML(fname);
00119                 case FILE_FORMAT_TEXT:
00120                 case FILE_FORMAT_DEFAULT:
00121                         return SaveText(fname);
00122                 default:
00123                         return false;
00124         }
00125 }
00126 
00127 bool MultiMarker::LoadXML(const char* fname) {
00128         TiXmlDocument document;
00129         if (!document.LoadFile(fname)) return false;
00130         TiXmlElement *xml_root = document.RootElement();
00131 
00132         int n_markers;
00133         if (xml_root->QueryIntAttribute("markers", &n_markers) != TIXML_SUCCESS) return false;
00134 
00135         pointcloud.clear();
00136         marker_indices.resize(n_markers);
00137         marker_status.resize(n_markers);
00138 
00139         TiXmlElement *xml_marker = xml_root->FirstChildElement("marker");
00140         for(int i = 0; i < n_markers; ++i) {
00141                 if (!xml_marker) return false;
00142 
00143                 int index, status;
00144                 if (xml_marker->QueryIntAttribute("index", &index) != TIXML_SUCCESS) return false;
00145                 if (xml_marker->QueryIntAttribute("status", &status) != TIXML_SUCCESS) return false;
00146                 marker_indices[i] = index;
00147                 marker_status[i] = status;
00148                 if(i==0) master_id = index;
00149 
00150                 TiXmlElement *xml_corner = xml_marker->FirstChildElement("corner");
00151                 for(int j = 0; j < 4; ++j) {
00152                         if (!xml_corner) return false;
00153 
00154                         CvPoint3D64f X;
00155                         if (xml_corner->QueryDoubleAttribute("x", &X.x) != TIXML_SUCCESS) return false;
00156                         if (xml_corner->QueryDoubleAttribute("y", &X.y) != TIXML_SUCCESS) return false;
00157                         if (xml_corner->QueryDoubleAttribute("z", &X.z) != TIXML_SUCCESS) return false;
00158                         pointcloud[pointcloud_index(marker_indices[i], j)] = X;
00159 
00160                         xml_corner = (TiXmlElement*)xml_corner->NextSibling("corner");
00161                 }
00162 
00163                 xml_marker = (TiXmlElement*)xml_marker->NextSibling("marker");
00164         }
00165         return true;
00166 }
00167 
00168 bool MultiMarker::LoadText(const char* fname) {
00169         fstream file_op(fname, ios::in);
00170 
00171     if (!file_op) {
00172         return false;
00173     }
00174 
00175         size_t n_markers;
00176         file_op>>n_markers;
00177 
00178         pointcloud.clear();
00179         marker_indices.resize(n_markers);
00180         marker_status.resize(n_markers);
00181 
00182         for(size_t i = 0; i < n_markers; ++i){
00183                 file_op>>marker_indices[i];
00184         }
00185 
00186         for(size_t i = 0; i < n_markers; ++i){
00187                 file_op>>marker_status[i];
00188         }
00189 
00190         for(size_t i = 0; i < n_markers; ++i)
00191                 for(size_t j = 0; j < 4; ++j)
00192                 {
00193                         CvPoint3D64f X;
00194                         file_op>>X.x;
00195                         file_op>>X.y;
00196                         file_op>>X.z;
00197                         pointcloud[pointcloud_index(marker_indices[i], j)] = X;
00198                 }
00199 
00200         file_op.close();
00201 
00202         return true;
00203 }
00204 
00205 bool MultiMarker::Load(const char* fname, FILE_FORMAT format) {
00206         switch (format) {
00207                 case FILE_FORMAT_XML:
00208                         return LoadXML(fname);
00209                 case FILE_FORMAT_TEXT:
00210                 case FILE_FORMAT_DEFAULT:
00211                         return LoadText(fname);
00212                 default:
00213                         return false;
00214         }
00215 }
00216 
00217 MultiMarker::MultiMarker(vector<int>& indices)
00218 {
00219         marker_indices.resize(indices.size());
00220         copy(indices.begin(), indices.end(), marker_indices.begin());
00221         marker_status.resize(indices.size());
00222         fill(marker_status.begin(), marker_status.end(), 0);
00223 }
00224 
00225 void MultiMarker::PointCloudReset() {
00226         pointcloud.clear();
00227 }
00228 
00229 void MultiMarker::PointCloudCorners3d(double edge_length, Pose &pose, CvPoint3D64f corners[4]) {
00230         // Transformation from origin to current marker
00231         CvMat *m3 = cvCreateMat(4,4,CV_64F); cvSetIdentity(m3);
00232         pose.GetMatrix(m3);
00233 
00234         for(size_t j = 0; j < 4; ++j)
00235         {
00236                 // TODO: This should be exactly the same as in Marker class.
00237                 //       Should we get the values from there somehow?
00238                 double X_data[4] = {0, 0, 0, 1};
00239                 if (j == 0) { 
00240                         int zzzz=2;                     
00241                         //X_data[0] = -0.5*edge_length;
00242                         //X_data[1] = -0.5*edge_length;
00243                 } else if (j == 1) {
00244                         X_data[0] = +0.5*edge_length;
00245                         X_data[1] = -0.5*edge_length;
00246                 } else if (j == 2) {
00247                         X_data[0] = +0.5*edge_length;
00248                         X_data[1] = +0.5*edge_length;
00249                 } else if (j == 3) {
00250                         X_data[0] = -0.5*edge_length;
00251                         X_data[1] = +0.5*edge_length;
00252                 }
00253 
00254                 CvMat X  = cvMat(4, 1, CV_64F, X_data);
00255                 cvMatMul(m3, &X, &X);
00256 
00257                 corners[j].x = X_data[0] / X_data[3];
00258                 corners[j].y = X_data[1] / X_data[3];
00259                 corners[j].z = X_data[2] / X_data[3];
00260         }
00261         cvReleaseMat(&m3);
00262 }
00263 
00264 void MultiMarker::PointCloudAdd(int marker_id, double edge_length, Pose &pose) {
00265         CvPoint3D64f corners[4];
00266         PointCloudCorners3d(edge_length, pose, corners);
00267         for(size_t j = 0; j < 4; ++j) {
00268                 pointcloud[pointcloud_index(marker_id, j, true)] = corners[j];
00269                 marker_status[get_id_index(marker_id, true)]=1;
00270         }
00271 }
00272 
00273 void MultiMarker::PointCloudCopy(const MultiMarker *m) {
00274         pointcloud.clear();
00275         pointcloud = m->pointcloud; // TODO: Is this copy operation ok?
00276         marker_indices.resize(m->marker_indices.size());
00277         marker_status.resize(m->marker_status.size());
00278         copy(m->marker_indices.begin(), m->marker_indices.end(), marker_indices.begin());
00279         copy(m->marker_status.begin(), m->marker_status.end(), marker_status.begin());
00280 }
00281 
00282 void MultiMarker::PointCloudGet(int marker_id, int point,
00283                                 double &x, double &y, double &z) {
00284   CvPoint3D64f p3d = pointcloud[pointcloud_index(marker_id, point)];
00285   x = p3d.x;
00286   y = p3d.y;
00287   z = p3d.z;
00288 }
00289 
00290 bool MultiMarker::IsValidMarker(int marker_id) {
00291         int idx = get_id_index(marker_id);
00292         return idx != -1 && marker_status[idx] != 0;
00293 }
00294 
00295 
00296 double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) {
00297         vector<CvPoint3D64f> world_points;
00298         vector<PointDouble>  image_points;
00299 
00300         // Reset the marker_status to 1 for all markers in point_cloud
00301         for (size_t i=0; i<marker_status.size(); i++) {
00302                 if (marker_status[i] > 0) marker_status[i]=1;
00303         }
00304 
00305         // For every detected marker
00306         for (MarkerIterator &i = begin.reset(); i != end; ++i)
00307         {
00308                 const Marker* marker = *i;
00309                 int id = marker->GetId();
00310                 int index = get_id_index(id);
00311                 if (index < 0) continue;
00312 
00313                 // But only if we have corresponding points in the pointcloud
00314                 if (marker_status[index] > 0) {
00315                         for(size_t j = 0; j < marker->marker_corners.size(); ++j)
00316                         {
00317                                 CvPoint3D64f Xnew = pointcloud[pointcloud_index(id, (int)j)];
00318                                 world_points.push_back(Xnew);
00319                                 image_points.push_back(marker->marker_corners_img.at(j));
00320                                 if (image) cvCircle(image, cvPoint(int(marker->marker_corners_img[j].x), int(marker->marker_corners_img[j].y)), 3, CV_RGB(0,255,0));
00321                         }
00322                         marker_status[index] = 2; // Used for tracking
00323                 }
00324         }
00325 
00326         if (world_points.size() < 4) return -1;
00327 
00328         double rod[3], tra[3];
00329         CvMat rot_mat = cvMat(3, 1,CV_64F, rod);
00330         CvMat tra_mat = cvMat(3, 1,CV_64F, tra);
00331         double error=0; // TODO: Now we don't calculate any error value
00332         cam->CalcExteriorOrientation(world_points, image_points, &rot_mat, &tra_mat);
00333         pose.SetRodriques(&rot_mat);
00334         pose.SetTranslation(&tra_mat);
00335         return error;
00336 }
00337 
00338 
00339 int MultiMarker::_SetTrackMarkers(MarkerDetectorImpl &marker_detector, Camera* cam, Pose& pose, IplImage *image) {
00340         int count=0;
00341         marker_detector.TrackMarkersReset();
00342         for(size_t i = 0; i < marker_indices.size(); ++i) {
00343                 int id = marker_indices[i];
00344                 // If the marker wasn't tracked lets add it to be trackable
00345                 if (marker_status[i] == 1) {
00346                         vector<CvPoint3D64f> pw(4);
00347                         pw[0] = pointcloud[pointcloud_index(id, 0)];
00348                         pw[1] = pointcloud[pointcloud_index(id, 1)];
00349                         pw[2] = pointcloud[pointcloud_index(id, 2)];
00350                         pw[3] = pointcloud[pointcloud_index(id, 3)];
00351                         vector<CvPoint2D64f> pi(4);
00352                         cam->ProjectPoints(pw, &pose, pi);
00353                         PointDouble p[4]; // TODO: This type copying is so silly!!!
00354                         p[0].x = pi[0].x;
00355                         p[0].y = pi[0].y;
00356                         p[1].x = pi[1].x;
00357                         p[1].y = pi[1].y;
00358                         p[2].x = pi[2].x;
00359                         p[2].y = pi[2].y;
00360                         p[3].x = pi[3].x;
00361                         p[3].y = pi[3].y;
00362                         if (image) {
00363                                 cvLine(image, cvPoint(int(p[0].x), int(p[0].y)), cvPoint(int(p[1].x), int(p[1].y)), CV_RGB(255,0,0));
00364                                 cvLine(image, cvPoint(int(p[1].x), int(p[1].y)), cvPoint(int(p[2].x), int(p[2].y)), CV_RGB(255,0,0));
00365                                 cvLine(image, cvPoint(int(p[2].x), int(p[2].y)), cvPoint(int(p[3].x), int(p[3].y)), CV_RGB(255,0,0));
00366                                 cvLine(image, cvPoint(int(p[3].x), int(p[3].y)), cvPoint(int(p[0].x), int(p[0].y)), CV_RGB(255,0,0));
00367                         }
00368                         marker_detector.TrackMarkerAdd(id, p);
00369                         count++;
00370                 }
00371         }
00372         return count;
00373 }
00374 
00375 }


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