MultiMarker.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"
27 #include <fstream>
28 
29 using namespace std;
30 
31 namespace alvar {
32 using namespace std;
33 
34 int MultiMarker::pointcloud_index(int marker_id, int marker_corner, bool add_if_missing /*=false*/) {
35  return (get_id_index(marker_id ,add_if_missing)*4)+marker_corner;
36 }
37 
38 int MultiMarker::get_id_index(int id, bool add_if_missing /*=false*/)
39 {
40  for(size_t i = 0; i < marker_indices.size(); ++i) {
41  if(marker_indices.at(i) == id)
42  return (int) i;
43  }
44  if (!add_if_missing) return -1;
45  marker_indices.push_back(id);
46  marker_status.push_back(0);
47  return (marker_indices.size()-1);
48 }
49 
50 void MultiMarker::Reset()
51 {
52  fill(marker_status.begin(), marker_status.end(), 0);
53  pointcloud.clear();
54 }
55 
56 bool MultiMarker::SaveXML(const char* fname) {
57  TiXmlDocument document;
58  document.LinkEndChild(new TiXmlDeclaration("1.0", "UTF-8", "no"));
59  document.LinkEndChild(new TiXmlElement("multimarker"));
60  TiXmlElement *xml_root = document.RootElement();
61 
62  int n_markers = marker_indices.size();
63  xml_root->SetAttribute("markers", n_markers);
64 
65  for(int i = 0; i < n_markers; ++i) {
66  TiXmlElement *xml_marker = new TiXmlElement("marker");
67  xml_root->LinkEndChild(xml_marker);
68 
69  xml_marker->SetAttribute("index", marker_indices[i]);
70  xml_marker->SetAttribute("status", marker_status[i]);
71 
72  for(int j = 0; j < 4; ++j) {
73  TiXmlElement *xml_corner = new TiXmlElement("corner");
74  xml_marker->LinkEndChild(xml_corner);
75  CvPoint3D64f X = pointcloud[pointcloud_index(marker_indices[i], j)];
76  xml_corner->SetDoubleAttribute("x", X.x);
77  xml_corner->SetDoubleAttribute("y", X.y);
78  xml_corner->SetDoubleAttribute("z", X.z);
79  }
80  }
81  return document.SaveFile(fname);
82 }
83 
84 bool MultiMarker::SaveText(const char* fname) {
85  size_t n_markers = marker_indices.size();
86 
87  fstream file_op(fname, ios::out);
88 
89  file_op<<n_markers<<endl;
90 
91  file_op<<endl;
92 
93  for(size_t i = 0; i < n_markers; ++i)
94  file_op<<marker_indices[i]<<endl;
95 
96  file_op<<endl;
97 
98  for(size_t i = 0; i < n_markers; ++i)
99  file_op<<marker_status[i]<<endl;
100 
101  file_op<<endl;
102 
103  for(size_t i = 0; i < n_markers; ++i)
104  for(size_t j = 0; j < 4; ++j)
105  {
106  CvPoint3D64f X = pointcloud[pointcloud_index(marker_indices[i], j)];
107  file_op<<X.x<<" "<<X.y<<" "<<X.z<<endl;
108 
109  }
110  file_op.close();
111 
112  return true;
113 }
114 
115 bool MultiMarker::Save(const char* fname, FILE_FORMAT format) {
116  switch (format) {
117  case FILE_FORMAT_XML:
118  return SaveXML(fname);
119  case FILE_FORMAT_TEXT:
120  case FILE_FORMAT_DEFAULT:
121  return SaveText(fname);
122  default:
123  return false;
124  }
125 }
126 
127 bool MultiMarker::LoadXML(const char* fname) {
128  TiXmlDocument document;
129  if (!document.LoadFile(fname)) return false;
130  TiXmlElement *xml_root = document.RootElement();
131 
132  int n_markers;
133  if (xml_root->QueryIntAttribute("markers", &n_markers) != TIXML_SUCCESS) return false;
134 
135  pointcloud.clear();
136  marker_indices.resize(n_markers);
137  marker_status.resize(n_markers);
138 
139  TiXmlElement *xml_marker = xml_root->FirstChildElement("marker");
140  for(int i = 0; i < n_markers; ++i) {
141  if (!xml_marker) return false;
142 
143  int index, status;
144  if (xml_marker->QueryIntAttribute("index", &index) != TIXML_SUCCESS) return false;
145  if (xml_marker->QueryIntAttribute("status", &status) != TIXML_SUCCESS) return false;
146  marker_indices[i] = index;
147  marker_status[i] = status;
148  if(i==0) master_id = index;
149 
150  TiXmlElement *xml_corner = xml_marker->FirstChildElement("corner");
151  for(int j = 0; j < 4; ++j) {
152  if (!xml_corner) return false;
153 
154  CvPoint3D64f X;
155  if (xml_corner->QueryDoubleAttribute("x", &X.x) != TIXML_SUCCESS) return false;
156  if (xml_corner->QueryDoubleAttribute("y", &X.y) != TIXML_SUCCESS) return false;
157  if (xml_corner->QueryDoubleAttribute("z", &X.z) != TIXML_SUCCESS) return false;
158  pointcloud[pointcloud_index(marker_indices[i], j)] = X;
159 
160  xml_corner = (TiXmlElement*)xml_corner->NextSibling("corner");
161  }
162 
163  xml_marker = (TiXmlElement*)xml_marker->NextSibling("marker");
164  }
165  return true;
166 }
167 
168 bool MultiMarker::LoadText(const char* fname) {
169  fstream file_op(fname, ios::in);
170 
171  if (!file_op) {
172  return false;
173  }
174 
175  size_t n_markers;
176  file_op>>n_markers;
177 
178  pointcloud.clear();
179  marker_indices.resize(n_markers);
180  marker_status.resize(n_markers);
181 
182  for(size_t i = 0; i < n_markers; ++i){
183  file_op>>marker_indices[i];
184  }
185 
186  for(size_t i = 0; i < n_markers; ++i){
187  file_op>>marker_status[i];
188  }
189 
190  for(size_t i = 0; i < n_markers; ++i)
191  for(size_t j = 0; j < 4; ++j)
192  {
193  CvPoint3D64f X;
194  file_op>>X.x;
195  file_op>>X.y;
196  file_op>>X.z;
197  pointcloud[pointcloud_index(marker_indices[i], j)] = X;
198  }
199 
200  file_op.close();
201 
202  return true;
203 }
204 
205 bool MultiMarker::Load(const char* fname, FILE_FORMAT format) {
206  switch (format) {
207  case FILE_FORMAT_XML:
208  return LoadXML(fname);
209  case FILE_FORMAT_TEXT:
210  case FILE_FORMAT_DEFAULT:
211  return LoadText(fname);
212  default:
213  return false;
214  }
215 }
216 
217 MultiMarker::MultiMarker(vector<int>& indices)
218 {
219  marker_indices.resize(indices.size());
220  copy(indices.begin(), indices.end(), marker_indices.begin());
221  marker_status.resize(indices.size());
222  fill(marker_status.begin(), marker_status.end(), 0);
223 }
224 
225 void MultiMarker::PointCloudReset() {
226  pointcloud.clear();
227 }
228 
229 void MultiMarker::PointCloudCorners3d(double edge_length, Pose &pose, CvPoint3D64f corners[4]) {
230  // Transformation from origin to current marker
231  CvMat *m3 = cvCreateMat(4,4,CV_64F); cvSetIdentity(m3);
232  pose.GetMatrix(m3);
233 
234  for(size_t j = 0; j < 4; ++j)
235  {
236  // TODO: This should be exactly the same as in Marker class.
237  // Should we get the values from there somehow?
238  double X_data[4] = {0, 0, 0, 1};
239  if (j == 0) {
240  int zzzz=2;
241  //X_data[0] = -0.5*edge_length;
242  //X_data[1] = -0.5*edge_length;
243  } else if (j == 1) {
244  X_data[0] = +0.5*edge_length;
245  X_data[1] = -0.5*edge_length;
246  } else if (j == 2) {
247  X_data[0] = +0.5*edge_length;
248  X_data[1] = +0.5*edge_length;
249  } else if (j == 3) {
250  X_data[0] = -0.5*edge_length;
251  X_data[1] = +0.5*edge_length;
252  }
253 
254  CvMat X = cvMat(4, 1, CV_64F, X_data);
255  cvMatMul(m3, &X, &X);
256 
257  corners[j].x = X_data[0] / X_data[3];
258  corners[j].y = X_data[1] / X_data[3];
259  corners[j].z = X_data[2] / X_data[3];
260  }
261  cvReleaseMat(&m3);
262 }
263 
264 void MultiMarker::PointCloudAdd(int marker_id, double edge_length, Pose &pose) {
265  CvPoint3D64f corners[4];
266  PointCloudCorners3d(edge_length, pose, corners);
267  for(size_t j = 0; j < 4; ++j) {
268  pointcloud[pointcloud_index(marker_id, j, true)] = corners[j];
269  marker_status[get_id_index(marker_id, true)]=1;
270  }
271 }
272 
273 void MultiMarker::PointCloudCopy(const MultiMarker *m) {
274  pointcloud.clear();
275  pointcloud = m->pointcloud; // TODO: Is this copy operation ok?
276  marker_indices.resize(m->marker_indices.size());
277  marker_status.resize(m->marker_status.size());
278  copy(m->marker_indices.begin(), m->marker_indices.end(), marker_indices.begin());
279  copy(m->marker_status.begin(), m->marker_status.end(), marker_status.begin());
280 }
281 
282 void MultiMarker::PointCloudGet(int marker_id, int point,
283  double &x, double &y, double &z) {
284  CvPoint3D64f p3d = pointcloud[pointcloud_index(marker_id, point)];
285  x = p3d.x;
286  y = p3d.y;
287  z = p3d.z;
288 }
289 
290 bool MultiMarker::IsValidMarker(int marker_id) {
291  int idx = get_id_index(marker_id);
292  return idx != -1 && marker_status[idx] != 0;
293 }
294 
295 
296 double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) {
297  vector<CvPoint3D64f> world_points;
298  vector<PointDouble> image_points;
299 
300  // Reset the marker_status to 1 for all markers in point_cloud
301  for (size_t i=0; i<marker_status.size(); i++) {
302  if (marker_status[i] > 0) marker_status[i]=1;
303  }
304 
305  // For every detected marker
306  for (MarkerIterator &i = begin.reset(); i != end; ++i)
307  {
308  const Marker* marker = *i;
309  int id = marker->GetId();
310  int index = get_id_index(id);
311  if (index < 0) continue;
312 
313  // But only if we have corresponding points in the pointcloud
314  if (marker_status[index] > 0) {
315  for(size_t j = 0; j < marker->marker_corners.size(); ++j)
316  {
317  CvPoint3D64f Xnew = pointcloud[pointcloud_index(id, (int)j)];
318  world_points.push_back(Xnew);
319  image_points.push_back(marker->marker_corners_img.at(j));
320  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));
321  }
322  marker_status[index] = 2; // Used for tracking
323  }
324  }
325 
326  if (world_points.size() < 4) return -1;
327 
328  double rod[3], tra[3];
329  CvMat rot_mat = cvMat(3, 1,CV_64F, rod);
330  CvMat tra_mat = cvMat(3, 1,CV_64F, tra);
331  double error=0; // TODO: Now we don't calculate any error value
332  cam->CalcExteriorOrientation(world_points, image_points, &rot_mat, &tra_mat);
333  pose.SetRodriques(&rot_mat);
334  pose.SetTranslation(&tra_mat);
335  return error;
336 }
337 
338 
339 int MultiMarker::_SetTrackMarkers(MarkerDetectorImpl &marker_detector, Camera* cam, Pose& pose, IplImage *image) {
340  int count=0;
341  marker_detector.TrackMarkersReset();
342  for(size_t i = 0; i < marker_indices.size(); ++i) {
343  int id = marker_indices[i];
344  // If the marker wasn't tracked lets add it to be trackable
345  if (marker_status[i] == 1) {
346  vector<CvPoint3D64f> pw(4);
347  pw[0] = pointcloud[pointcloud_index(id, 0)];
348  pw[1] = pointcloud[pointcloud_index(id, 1)];
349  pw[2] = pointcloud[pointcloud_index(id, 2)];
350  pw[3] = pointcloud[pointcloud_index(id, 3)];
351  vector<CvPoint2D64f> pi(4);
352  cam->ProjectPoints(pw, &pose, pi);
353  PointDouble p[4]; // TODO: This type copying is so silly!!!
354  p[0].x = pi[0].x;
355  p[0].y = pi[0].y;
356  p[1].x = pi[1].x;
357  p[1].y = pi[1].y;
358  p[2].x = pi[2].x;
359  p[2].y = pi[2].y;
360  p[3].x = pi[3].x;
361  p[3].y = pi[3].y;
362  if (image) {
363  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));
364  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));
365  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));
366  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));
367  }
368  marker_detector.TrackMarkerAdd(id, p);
369  count++;
370  }
371  }
372  return count;
373 }
374 
375 }
Main ALVAR namespace.
Definition: Alvar.h:174
Default file format.
Definition: FileFormat.h:45
Plain ASCII text file format.
Definition: FileFormat.h:59
std::vector< PointDouble > marker_corners_img
Marker corners in image coordinates.
Definition: Marker.h:177
virtual unsigned long GetId() const
Get id for this marker This is used e.g. in MarkerDetector to associate a marker id with an appropria...
Definition: Marker.h:118
std::vector< int > marker_status
Definition: MultiMarker.h:65
unsigned char * image
Definition: GlutViewer.cpp:155
int * master_id
Base class for using MultiMarker.
Definition: MultiMarker.h:47
TFSIMD_FORCE_INLINE const tfScalar & y() const
Templateless version of MarkerDetector. Please use MarkerDetector instead.
std::map< int, CvPoint3D64f > pointcloud
Definition: MultiMarker.h:63
std::vector< int > marker_indices
Definition: MultiMarker.h:64
void TrackMarkersReset()
Clear the markers that are tracked.
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
FILE_FORMAT
Definition: FileFormat.h:39
void ProjectPoints(std::vector< CvPoint3D64f > &pw, Pose *pose, std::vector< CvPoint2D64f > &pi) const
Project points.
This file implements utilities that assist in reading and writing files.
Pose representation derived from the Rotation class
Definition: Pose.h:50
TFSIMD_FORCE_INLINE const tfScalar & x() const
Basic 2D Marker functionality.
Definition: Marker.h:52
XML file format.
Definition: FileFormat.h:66
void SetRodriques(const CvMat *mat)
Sets the rotation from given rotation vector.
Definition: Rotation.cpp:314
MarkerDetector< MarkerData > marker_detector
Iterator type for traversing templated Marker vector without the template.
Definition: Marker.h:277
This file implements a multi-marker.
void GetMatrix(CvMat *mat) const
Definition: Pose.cpp:91
ALVAR_EXPORT Point< CvPoint2D64f > PointDouble
The default double point type.
Definition: Util.h:108
void CalcExteriorOrientation(std::vector< CvPoint3D64f > &pw, std::vector< CvPoint2D64f > &pi, Pose *pose)
Calculate exterior orientation.
void TrackMarkerAdd(int id, PointDouble corners[4])
Add markers to be tracked Sometimes application or e.g. the MultiMarker implementation knows more abo...
Camera * cam
void SetTranslation(const CvMat *tra)
Definition: Pose.cpp:150
virtual MarkerIterator & reset()=0
This file defines library export definitions, version numbers and build information.
std::vector< PointDouble > marker_corners
Marker corners in marker coordinates.
Definition: Marker.h:175


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:24