SampleMultiMarkerBundle.cpp
Go to the documentation of this file.
1 #include "CvTestbed.h"
2 #include "MarkerDetector.h"
3 #include "MultiMarkerBundle.h"
5 #include "Pose.h"
6 #include "Shared.h"
7 using namespace alvar;
8 using namespace std;
9 
10 int visualize=1;
11 const int nof_markers = 8;
12 const double marker_size = 4;
13 bool add_measurement=false;
14 bool optimize = false;
15 bool optimize_done = false;
17 std::stringstream calibrationFilename;
18 
21 double GetMultiMarkerPose(IplImage *image, Camera *cam, Pose &pose) {
22  static bool init=true;
23  if (init) {
24  cout<<"Using manual multimarker approach with MultiMarkerInitializer & MultiMarkerBundle."<<endl;
25  cout<<"Use 'p' for taking keyframes and 'o' for optimizing."<<endl;
26  init=false;
27  vector<int> id_vector;
28  for(int i = 0; i < nof_markers; ++i)
29  id_vector.push_back(i);
30  // We make the initialization for MultiMarkerBundle using MultiMarkerInitializer
31  // Each marker needs to be visible in at least two images and at most 64 image are used.
32  multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64);
33  pose.Reset();
34  multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose);
35  multi_marker_bundle = new MultiMarkerBundle(id_vector);
36  }
37 
38  double error = -1;
39  if (!optimize_done) {
40  if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
41  if (visualize == 2)
42  error = multi_marker_init->Update(marker_detector.markers, cam, pose, image);
43  else
44  error = multi_marker_init->Update(marker_detector.markers, cam, pose);
45  }
46  } else {
47  if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
48  if (visualize == 2)
49  error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
50  else
51  error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
52  if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) &&
53  (marker_detector.DetectAdditional(image, cam, (visualize == 3)) > 0))
54  {
55  if (visualize == 3)
56  error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
57  else
58  error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
59  }
60  }
61  }
62 
63  if (add_measurement) {
64  if (marker_detector.markers->size() >= 2) {
65  cout<<"Adding measurement..."<<endl;
66  multi_marker_init->MeasurementsAdd(marker_detector.markers);
67  add_measurement=false;
68  }
69  }
70 
71  if (optimize) {
72  cout<<"Initializing..."<<endl;
73  if (!multi_marker_init->Initialize(cam)) {
74  cout<<"Initialization failed, add some more measurements."<<endl;
75 
76  } else {
77  // Reset the bundle adjuster.
78  multi_marker_bundle->Reset();
79  multi_marker_bundle->MeasurementsReset();
80  // Copy all measurements into the bundle adjuster.
81  for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) {
82  Pose pose;
83  multi_marker_init->getMeasurementPose(i, cam, pose);
84  const std::vector<MultiMarkerInitializer::MarkerMeasurement> markers
85  = multi_marker_init->getMeasurementMarkers(i);
86  multi_marker_bundle->MeasurementsAdd(&markers, pose);
87  }
88  // Initialize the bundle adjuster with initial marker poses.
89  multi_marker_bundle->PointCloudCopy(multi_marker_init);
90  cout<<"Optimizing..."<<endl;
91  if (multi_marker_bundle->Optimize(cam, 0.01, 20)) {
92  cout<<"Optimizing done"<<endl;
93  optimize_done=true;
94 
95  } else {
96  cout<<"Optimizing FAILED!"<<endl;
97  }
98  }
99  optimize=false;
100  }
101  return error;
102 }
103 
104 void videocallback(IplImage *image)
105 {
106  static Camera cam;
107  static Pose pose;
108  bool flip_image = (image->origin?true:false);
109  if (flip_image) {
110  cvFlip(image);
111  image->origin = !image->origin;
112  }
113 
114  static bool init = true;
115  if (init)
116  {
117  init = false;
118 
119  // Initialize camera
120  cout<<"Loading calibration: "<<calibrationFilename.str();
121 
122  if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height))
123  {
124  cout<<" [Ok]"<<endl;
125  }
126  else
127  {
128  cam.SetRes(image->width, image->height);
129  cout<<" [Fail]"<<endl;
130  }
131 
132  marker_detector.SetMarkerSize(marker_size);
133  }
134 
135  // Manual approach (use 'p' for taking keyframes and 'o' for optimizing)
136  double error = GetMultiMarkerPose(image, &cam, pose);
137 
138  // Visualize a blue marker at the origo.
139  static Marker foo;
141  if ((error >= 0) && (error < 5))
142  {
143  foo.pose = pose;
144  }
145  foo.Visualize(image, &cam, CV_RGB(0,0,255));
146 
147  if (flip_image) {
148  cvFlip(image);
149  image->origin = !image->origin;
150  }
151 }
152 
153 int keycallback(int key)
154 {
155  static bool fixed = false;
156 
157  if(key == 'r')
158  {
159  cout<<"Reseting multi marker"<<endl;
160  multi_marker_init->Reset();
161  multi_marker_init->MeasurementsReset();
162  multi_marker_bundle->Reset();
163  multi_marker_bundle->MeasurementsReset();
164  add_measurement = false;
165  optimize = false;
166  optimize_done = false;
167  }
168  else if(key == 'v')
169  {
170  visualize = (visualize+1)%3;
171  }
172  else if(key == 'l')
173  {
174  if(multi_marker_bundle->Load("mmarker.xml", FILE_FORMAT_XML))
175  {
176  cout<<"Multi marker loaded"<<endl;
177  multi_marker_init->PointCloudCopy(multi_marker_bundle);
178  optimize_done = true;
179  }
180  else
181  cout<<"Cannot load multi marker"<<endl;
182  }
183  else if(key == 's')
184  {
185  if(multi_marker_bundle->Save("mmarker.xml", FILE_FORMAT_XML))
186  cout<<"Multi marker saved"<<endl;
187  else
188  cout<<"Cannot save multi marker"<<endl;
189  }
190  else if(key == 'p')
191  {
192  add_measurement=true;
193  }
194  else if(key == 'o')
195  {
196  optimize=true;
197  }
198  else return key;
199 
200  return 0;
201 }
202 
203 int main(int argc, char *argv[])
204 {
205  try {
206  // Output usage message
207  std::string filename(argv[0]);
208  filename = filename.substr(filename.find_last_of('\\') + 1);
209  std::cout << "SampleMultiMarkerBundle" << std::endl;
210  std::cout << "=======================" << std::endl;
211  std::cout << std::endl;
212  std::cout << "Description:" << std::endl;
213  std::cout << " This is an example of how to use the 'MultiMarkerBundle' class to" << std::endl;
214  std::cout << " automatically deduce and optimize 'MultiMarker' setups." << std::endl;
215  std::cout << std::endl;
216  std::cout << "Usage:" << std::endl;
217  std::cout << " " << filename << " [device]" << std::endl;
218  std::cout << std::endl;
219  std::cout << " device integer selecting device from enumeration list (default 0)" << std::endl;
220  std::cout << " highgui capture devices are prefered" << std::endl;
221  std::cout << std::endl;
222  std::cout << "Keyboard Shortcuts:" << std::endl;
223  std::cout << " l: load marker configuration from mmarker.txt" << std::endl;
224  std::cout << " s: save marker configuration to mmarker.txt" << std::endl;
225  std::cout << " r: reset marker configuration" << std::endl;
226  std::cout << " p: add measurement" << std::endl;
227  std::cout << " o: optimize bundle" << std::endl;
228  std::cout << " q: quit" << std::endl;
229  std::cout << std::endl;
230 
231  // Initialise CvTestbed
234 
235  // Enumerate possible capture plugins
237  if (plugins.size() < 1) {
238  std::cout << "Could not find any capture plugins." << std::endl;
239  return 0;
240  }
241 
242  // Display capture plugins
243  std::cout << "Available Plugins: ";
244  outputEnumeratedPlugins(plugins);
245  std::cout << std::endl;
246 
247  // Enumerate possible capture devices
249  if (devices.size() < 1) {
250  std::cout << "Could not find any capture devices." << std::endl;
251  return 0;
252  }
253 
254  // Check command line argument for which device to use
255  int selectedDevice = defaultDevice(devices);
256  if (argc > 1) {
257  selectedDevice = atoi(argv[1]);
258  }
259  if (selectedDevice >= (int)devices.size()) {
260  selectedDevice = defaultDevice(devices);
261  }
262 
263  // Display capture devices
264  std::cout << "Enumerated Capture Devices:" << std::endl;
265  outputEnumeratedDevices(devices, selectedDevice);
266  std::cout << std::endl;
267 
268  // Create capture object from camera
269  Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
270  std::string uniqueName = devices[selectedDevice].uniqueName();
271 
272  // Handle capture lifecycle and start video capture
273  // Note that loadSettings/saveSettings are not supported by all plugins
274  if (cap) {
275  std::stringstream settingsFilename;
276  settingsFilename << "camera_settings_" << uniqueName << ".xml";
277  calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
278 
279  cap->start();
280  cap->setResolution(640, 480);
281 
282  if (cap->loadSettings(settingsFilename.str())) {
283  std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
284  }
285 
286  std::stringstream title;
287  title << "SampleMultiMarkerBundle (" << cap->captureDevice().captureType() << ")";
288 
289  CvTestbed::Instance().StartVideo(cap, title.str().c_str());
290 
291  if (cap->saveSettings(settingsFilename.str())) {
292  std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
293  }
294 
295  cap->stop();
296  delete cap;
297  }
298  else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
299  }
300  else {
301  std::cout << "Could not initialize the selected capture backend." << std::endl;
302  }
303 
304  return 0;
305  }
306  catch (const std::exception &e) {
307  std::cout << "Exception: " << e.what() << endl;
308  }
309  catch (...) {
310  std::cout << "Exception: unknown" << std::endl;
311  }
312 }
double getMeasurementPose(int measurement, Camera *cam, Pose &pose)
const int nof_markers
Main ALVAR namespace.
Definition: Alvar.h:174
static CvTestbed & Instance()
The one and only instance of CvTestbed is accessed using CvTestbed::Instance()
Definition: CvTestbed.cpp:88
filename
Capture * createCapture(const CaptureDevice captureDevice)
Create Capture class. Transfers onwership to the caller.
virtual void Reset()
Resets the multi marker.
Definition: MultiMarker.cpp:50
bool add_measurement
virtual void stop()=0
Stops the camera capture.
void MeasurementsAdd(const std::vector< M, Eigen::aligned_allocator< M > > *markers, const Pose &camera_pose)
Adds new measurements that are used in bundle adjustment.
virtual bool start()=0
Starts the camera capture.
void SetMarkerSize(double _edge_length=1, int _res=5, double _margin=2)
CaptureDevice captureDevice()
The camera information associated to this capture object.
Definition: Capture.h:70
void SetVideoCallback(void(*_videocallback)(IplImage *image))
Set the videocallback function that will be called for every frame.
Definition: CvTestbed.cpp:93
void Visualize(IplImage *image, Camera *cam, CvScalar color=CV_RGB(255, 0, 0)) const
Visualize the marker.
Definition: Marker.cpp:134
static CaptureFactory * instance()
The singleton instance of CaptureFactory.
This file implements a generic marker detector.
void SetRes(int _x_res, int _y_res)
If we have no calibration file we can still adjust the default calibration to current resolution...
Definition: Camera.cpp:374
bool Optimize(Camera *_cam, double stop, int max_iter, Optimization::OptimizeMethod method=Optimization::TUKEY_LM)
Runs the bundle adjustment optimization.
unsigned char * image
Definition: GlutViewer.cpp:155
Initializes multi marker by estimating their relative positions from one or more images.
std::vector< CaptureDevice > CaptureDeviceVector
Vector of CaptureDevices.
int DetectAdditional(IplImage *image, Camera *cam, bool visualize=false, double max_track_error=0.2)
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
std::string captureType() const
The type of capture backend.
bool StartVideo(Capture *_cap, const char *_wintitle=0)
Start video input from given capture device.
Definition: CvTestbed.cpp:101
bool SetCalib(const char *calibfile, int _x_res, int _y_res, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Set the calibration file and the current resolution for which the calibration is adjusted to...
Definition: Camera.cpp:270
std::vector< M, Eigen::aligned_allocator< M > > * markers
std::vector< std::string > CapturePluginVector
Vector of strings.
const double marker_size
void outputEnumeratedPlugins(CaptureFactory::CapturePluginVector &plugins)
Definition: Shared.h:8
MarkerDetector for detecting markers of type M
void SetKeyCallback(int(*_keycallback)(int key))
Sets the keyboard callback function that will be called when keyboard is pressed. ...
Definition: CvTestbed.cpp:97
Pose pose
The current marker Pose.
Definition: Marker.h:136
MarkerDetector< MarkerData > marker_detector
std::stringstream calibrationFilename
void Reset()
Definition: Pose.cpp:69
CaptureDeviceVector enumerateDevices(const std::string &captureType="")
Enumerate capture devices currently available.
Multi marker that uses bundle adjustment to refine the 3D positions of the markers (point cloud)...
void PointCloudCopy(const MultiMarker *m)
Copies the 3D point cloud from other multi marker object.
Pose representation derived from the Rotation class
Definition: Pose.h:50
CapturePluginVector enumeratePlugins()
Enumerate capture plugins currently available.
Basic 2D Marker functionality.
Definition: Marker.h:52
XML file format.
Definition: FileFormat.h:66
void PointCloudAdd(int marker_id, double edge_length, Pose &pose)
Adds marker corners to 3D point cloud of multi marker.
bool init
void outputEnumeratedDevices(CaptureFactory::CaptureDeviceVector &devices, int selectedDevice)
Definition: Shared.h:20
void MeasurementsAdd(MarkerIterator &begin, MarkerIterator &end)
bool optimize_done
This file implements a initialization algorithm to create a multi-marker field configuration.
void videocallback(IplImage *image)
bool Load(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Loads multi marker configuration from a text file.
MultiMarkerInitializer * multi_marker_init
Capture interface that plugins must implement.
Definition: Capture.h:46
void SetMarkerSize(double _edge_length=0, int _res=0, double _margin=0)
Method for resizing the marker dimensions.
Definition: Marker.cpp:356
int Detect(IplImage *image, Camera *cam, bool track=false, bool visualize=false, double max_new_marker_error=0.08, double max_track_error=0.2, LabelingMethod labeling_method=CVSEQ, bool update_pose=true)
Detect Marker &#39;s from image
int main(int argc, char *argv[])
double GetMultiMarkerPose(IplImage *image, Camera *cam, Pose &pose)
This file implements a pose.
virtual bool loadSettings(std::string filename)
Load camera settings from a file.
Definition: Capture.h:145
int defaultDevice(CaptureFactory::CaptureDeviceVector &devices)
Definition: Shared.h:40
Camera * cam
int keycallback(int key)
virtual void setResolution(const unsigned long xResolution, const unsigned long yResolution)
Set the resolution.
Definition: Capture.h:93
This file implements an algorithm to create a multi-marker field configuration.
void MeasurementsReset()
Resets the measurements and camera poses that are stored for bundle adjustment. If something goes fro...
int SetTrackMarkers(MarkerDetector< M > &marker_detector, Camera *cam, Pose &pose, IplImage *image=0)
Set new markers to be tracked for MarkerDetector Sometimes the MultiMarker implementation knows more ...
Definition: MultiMarker.h:191
const std::vector< MarkerMeasurement, Eigen::aligned_allocator< MarkerMeasurement > > & getMeasurementMarkers(int measurement)
double Update(const std::vector< M, Eigen::aligned_allocator< M > > *markers, Camera *cam, Pose &pose, IplImage *image=0)
Calls GetPose to obtain camera pose.
Definition: MultiMarker.h:120
virtual bool saveSettings(std::string filename)
Save camera settings to a file.
Definition: Capture.h:124
bool Save(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Saves multi marker configuration to a text file.
MultiMarkerBundle * multi_marker_bundle


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