MultiMarkerInitializer.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 
25 #include <Eigen/StdVector>
26 
27 using namespace std;
28 
29 namespace alvar {
30 using namespace std;
31 
32 MultiMarkerInitializer::MultiMarkerInitializer(std::vector<int>& indices, int _filter_buffer_min, int _filter_buffer_max)
33  : MultiMarker(indices), filter_buffer_min(_filter_buffer_min) {
34 
35  marker_detected.resize(indices.size());
36  pointcloud_filtered = new FilterMedian[indices.size()*4*3];
37  for (size_t i=0; i<indices.size()*4*3; i++) {
38  pointcloud_filtered[i].setWindowSize(_filter_buffer_max);
39  }
40 
42 }
43 
45  // copy markers into measurements.
46  vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> > new_measurements;
47  for (MarkerIterator &i = begin.reset(); i != end; ++i) {
48  const Marker* marker = *i;
49  int index = get_id_index(marker->GetId());
50  if (index == -1) continue;
52  m.SetId(marker->GetId());
53  m.SetMarkerSize(marker->GetMarkerEdgeLength(), marker->GetRes(), marker->GetMargin());
54  m.pose = marker->pose;
55  m.marker_corners_img = i->marker_corners_img;
56  new_measurements.push_back(m);
57  marker_detected[index] = true;
58  }
59 
60  // If we have seen the 0 marker the first time we push it into point could.
61  for (MarkerIterator &i = begin.reset(); i != end; ++i) {
62  const Marker* marker = *i;
63  int id = marker->GetId();
64  int index = get_id_index(id);
65  // Initialize marker pose
66  //if (id == 0 && marker_status[index] == 0)
67  if (index == 0 && marker_status[index] == 0)
68  {
69  Pose pose;
70  CvPoint3D64f corners[4];
71  PointCloudCorners3d(marker->GetMarkerEdgeLength(), pose, corners);
72  for(size_t j = 0; j < 4; ++j) {
73  int p_index = pointcloud_index(id, j);
74  pointcloud[p_index] = corners[j];
75  }
76  marker_status[index] = 1;
77  }
78  }
79 
80  measurements.push_back(new_measurements);
81 }
82 
84  measurements.clear();
86  fill(marker_status.begin(), marker_status.end(), 0);
87  fill(marker_detected.begin(), marker_detected.end(), false);
88 
89  for (size_t i=0; i<marker_indices.size()*4*3; i++) {
91  }
92 }
93 
95  for (bool found_new = true; found_new; ) {
96  found_new = false;
97  // Iterate through all measurements, try to compute Pose for each.
98  for (MeasurementIterator mi = measurements.begin(); mi != measurements.end(); ++mi) {
99  vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> > &markers = *mi;
100  Pose pose;
101  MarkerIteratorImpl<MarkerMeasurement> m_begin(markers.begin());
102  MarkerIteratorImpl<MarkerMeasurement> m_end(markers.end());
103  double err = _GetPose(m_begin, m_end, cam, pose, NULL);
104  if (err >= 0) {
105  // If pose is found, estimate marker poses for those that are still unkown.
106  found_new = updateMarkerPoses(markers, pose);
107  }
108  }
109  }
110 
111  // Check if every marker has been seen
112  int n_detected = 0;
113  for (unsigned int i = 0; i < marker_indices.size(); ++i) {
114  cout << i << " " << marker_detected[i] << " " << marker_status[i] << "\n";
115  if (marker_detected[i] && marker_status[i] != 0) ++n_detected;
116  }
117  return n_detected;
118 }
119 
120 bool MultiMarkerInitializer::updateMarkerPoses(vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> > &markers, const Pose &pose) {
121  bool found_new = false;
122  for (vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> >::iterator i = markers.begin(); i != markers.end(); ++i) {
123  MarkerMeasurement &marker = *i;
124  int id = marker.GetId();
125  int index = get_id_index(id);
126  if (index > 0 && !marker.globalPose) {
127  // Compute absolute marker pose.
128  double cam_posed[16];
129  double mar_posed[16];
130  CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed);
131  CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed);
132  pose.GetMatrix(&cam_mat);
133  marker.pose.GetMatrix(&mar_mat);
134  cvInvert(&cam_mat, &cam_mat);
135  cvMatMul(&cam_mat, &mar_mat, &mar_mat);
136  marker.pose.SetMatrix(&mar_mat);
137  // Put marker into point cloud
138  CvPoint3D64f corners[4];
139  PointCloudCorners3d(marker.GetMarkerEdgeLength(), marker.pose, corners);
140  for(size_t j = 0; j < 4; ++j) {
141  CvPoint3D64f p;
142  int p_index = pointcloud_index(id, j);
143  p.x = pointcloud_filtered[3*p_index+0].next(corners[j].x);
144  p.y = pointcloud_filtered[3*p_index+1].next(corners[j].y);
145  p.z = pointcloud_filtered[3*p_index+2].next(corners[j].z);
146  if (pointcloud_filtered[3*p_index+0].getCurrentSize() >= filter_buffer_min) {
147  pointcloud[p_index] = p;
148  marker_status[index] = 1;
149  }
150  }
151  marker.globalPose = 1;
152  found_new = true;
153  }
154  }
155  return found_new;
156 }
157 
158 } // namespace alvar
Main ALVAR namespace.
Definition: Alvar.h:174
void PointCloudCorners3d(double edge_length, Pose &pose, CvPoint3D64f corners[4])
Calculates 3D coordinates of marker corners relative to given pose (camera).
int GetRes() const
Definition: Marker.h:126
void SetMatrix(const CvMat *mat)
Definition: Pose.cpp:75
std::vector< PointDouble > marker_corners_img
Marker corners in image coordinates.
Definition: Marker.h:177
unsigned long GetId() const
Get id for this marker This is used e.g. in MarkerDetector to associate a marker id with an appropria...
void setWindowSize(int size)
Definition: Filter.h:137
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
bool updateMarkerPoses(std::vector< MarkerMeasurement, Eigen::aligned_allocator< MarkerMeasurement > > &markers, const Pose &pose)
Base class for using MultiMarker.
Definition: MultiMarker.h:47
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::map< int, CvPoint3D64f > pointcloud
Definition: MultiMarker.h:63
int get_id_index(int id, bool add_if_missing=false)
Definition: MultiMarker.cpp:38
std::vector< int > marker_indices
Definition: MultiMarker.h:64
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
virtual void reset()
Reset the filter state.
Definition: Filter.cpp:63
std::vector< std::vector< MarkerMeasurement, Eigen::aligned_allocator< MarkerMeasurement > > >::iterator MeasurementIterator
virtual double next(double y)
Update the value. All inherited classes need to update value in next().
Definition: Filter.cpp:78
int pointcloud_index(int marker_id, int marker_corner, bool add_if_missing=false)
Definition: MultiMarker.cpp:34
Pose pose
The current marker Pose.
Definition: Marker.h:136
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
MarkerMeasurement that holds the maker id.
std::vector< std::vector< MarkerMeasurement, Eigen::aligned_allocator< MarkerMeasurement > > > measurements
void PointCloudReset()
Reserts the 3D point cloud of the markers.
TFSIMD_FORCE_INLINE const tfScalar & z() const
Iterator type for traversing templated Marker vector without the template.
Definition: Marker.h:277
double GetMarkerEdgeLength() const
Get edge length (to support different size markers.
Definition: Marker.h:102
void MeasurementsAdd(MarkerIterator &begin, MarkerIterator &end)
FilterMedian provides an median filter
Definition: Filter.h:133
void GetMatrix(CvMat *mat) const
Definition: Pose.cpp:91
This file implements a initialization algorithm to create a multi-marker field configuration.
Iterator implementation for traversing templated Marker vector without the template.
Definition: Marker.h:294
void SetMarkerSize(double _edge_length=0, int _res=0, double _margin=0)
Method for resizing the marker dimensions.
Definition: Marker.cpp:356
double _GetPose(MarkerIterator &begin, MarkerIterator &end, Camera *cam, Pose &pose, IplImage *image)
Camera * cam
double GetMargin() const
Definition: Marker.h:132
virtual MarkerIterator & reset()=0


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