My Project
EC.h
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 #ifndef EC_H
25 #define EC_H
26 
34 #include "Camera.h"
35 #include "MarkerDetector.h"
36 #include "MultiMarker.h"
37 #include "TrackerFeatures.h"
38 
39 namespace alvar {
40 
153 {
154 public:
155  int type_id;
156  bool has_p2d;
157  bool has_p3d;
158  cv::Point2f p2d;
159  cv::Point3f p3d;
160  cv::Point2f
161  projected_p2d; // This is only temporary -- user needs to take care that it has valid content
162  ExternalContainer() : type_id(-1), has_p2d(false), has_p3d(false)
163  {
164  }
166  {
167  type_id = c.type_id;
168  has_p2d = c.has_p2d;
169  has_p3d = c.has_p3d;
170  p2d.x = c.p2d.x;
171  p2d.y = c.p2d.y;
172  p3d.x = c.p3d.x;
173  p3d.y = c.p3d.y;
174  p3d.z = c.p3d.z;
175  projected_p2d.x = c.projected_p2d.x;
176  projected_p2d.y = c.projected_p2d.y;
177  }
178 };
179 
186 template <typename T>
188 {
189 protected:
190  int type_id;
191  bool needs_has_p2d;
192  bool needs_has_p3d;
193 
194 public:
195  DoHandleTest(int _type_id = -1, bool _needs_has_p2d = false, bool _needs_has_p3d = false)
196  : type_id(_type_id), needs_has_p2d(_needs_has_p2d), needs_has_p3d(_needs_has_p3d)
197  {
198  }
199  virtual bool
200  operator()(const T &f) const
201  {
202  if (needs_has_p2d && !f.has_p2d)
203  return false;
204  if (needs_has_p3d && !f.has_p3d)
205  return false;
206  // if (f.type_id == -1) return false; // Unnecessary?
207  if ((type_id != -1) && (type_id != f.type_id))
208  return false;
209  return true;
210  }
211 };
212 
218 template <typename T>
220 {
221 protected:
222  int type_id;
223  bool erase_without_p2d;
224  bool erase_without_p3d;
225  bool test_reprojection;
226  float limit_sq;
227 
228 public:
229  DoEraseTest(bool _erase_without_p2d, bool _erase_without_p3d, int _type_id = -1)
230  : type_id(_type_id),
231  erase_without_p2d(_erase_without_p2d),
232  erase_without_p3d(_erase_without_p3d),
233  test_reprojection(false),
234  limit_sq(0.f)
235  {
236  }
237  DoEraseTest(float _limit,
238  bool _erase_without_p2d = false,
239  bool _erase_without_p3d = false,
240  int _type_id = -1)
241  : type_id(_type_id),
242  erase_without_p2d(_erase_without_p2d),
243  erase_without_p3d(_erase_without_p3d),
244  test_reprojection(true),
245  limit_sq(_limit * _limit)
246  {
247  }
248  virtual bool
249  operator()(const T &f) const
250  {
251  if ((type_id != -1) && (type_id != f.type_id))
252  return false;
253  if (erase_without_p2d && !f.has_p2d)
254  return true;
255  if (erase_without_p3d && !f.has_p3d)
256  return true;
257  if (test_reprojection) {
258  if (!f.has_p2d)
259  return false;
260  if (!f.has_p3d)
261  return false;
262  // Note that the projected_p2d needs to have valid content at this point
263  double dist_sq = (f.p2d.x - f.projected_p2d.x) * (f.p2d.x - f.projected_p2d.x);
264  dist_sq += (f.p2d.y - f.projected_p2d.y) * (f.p2d.y - f.projected_p2d.y);
265  if (dist_sq > limit_sq)
266  return true;
267  }
268  return false;
269  }
270 };
271 
273 template <typename T, typename F>
274 inline int
275 EraseItemsEC(std::map<int, T> &container, F do_erase_test)
276 {
277  int count = 0;
278  typename std::map<int, T>::iterator iter = container.begin();
279  while (iter != container.end()) {
280  T &f = iter->second;
281  if (do_erase_test(f)) {
282  container.erase(iter++);
283  count++;
284  } else
285  iter++;
286  }
287  return count;
288 }
289 
292 {
293 protected:
294  bool purge;
295 
296 public:
298  TrackerFeaturesEC(int _max_features = 100,
299  int _min_features = 90,
300  double _quality_level = 0.01,
301  double _min_distance = 10,
302  int _pyr_levels = 4,
303  int win_size = 6)
304  : TrackerFeatures(_max_features,
305  _min_features,
306  _quality_level,
307  _min_distance,
308  _pyr_levels,
309  win_size),
310  purge(false)
311  {
312  }
313 
315  template <typename T>
316  bool
317  Track(cv::Mat & img,
318  cv::Mat & mask,
319  std::map<int, T> &container,
320  int type_id = -1,
321  int first_id = -1,
322  int last_id = -1)
323  {
324  DoHandleTest<T> do_handle_test_default(type_id);
325  if (type_id == -1)
326  type_id = 0;
327  return Track(img, mask, container, do_handle_test_default, type_id, first_id, last_id);
328  }
329 
331  /*template<typename T, typename F>
332  bool Track(cv::Mat&img, cv::Mat&mask, std::map<int,T> &container, F do_handle_test, int type_id=0, int first_id=0, int last_id=65535)
333  {
334  // Update features to match the ones in the given container
335  typename std::map<int,T>::iterator iter = container.begin();
336  typename std::map<int,T>::iterator iter_end = container.end();
337  feature_count = 0;
338  for (;iter != iter_end; iter++) {
339  T &f = iter->second;
340  if (!do_handle_test(f)) continue;
341  if (f.has_p2d != true) continue;
342  f.has_p2d = false; // This is updated again to true if tracking succeeds
343  features[feature_count] = f.p2d;
344  ids[feature_count] = iter->first;
345  feature_count++;
346  if (feature_count == max_features) break;
347  }
348  // Check that next_id is ok
349  if (next_id < first_id) next_id = first_id;
350  if (next_id > last_id) return false; // TODO: Make some better solution for this
351  // Purge if needed
352  if (purge) {
353  TrackerFeatures::Purge();
354  purge=false;
355  }
356  // Track as usual (this will swap above features to prev_features)
357  TrackHid(img, mask);
358  // Update the container to have the updated features
359  for (int i=0; i<feature_count; i++) {
360  int id = ids[i];
361  if (id >= last_id) break;
362  T &f = container[id];
363  f.type_id = type_id;
364  f.has_p2d = true;
365  f.p2d = features[i];
366  }
367  return true;
368  }*/
369 
371  template <typename T, typename F>
372  bool
373  Track(cv::Mat & img,
374  cv::Mat & mask,
375  std::map<int, T> &container,
376  F do_handle_test,
377  int type_id = 0,
378  int first_id = -1,
379  int last_id = -1)
380  {
381  // When first_id && last_id are < 0 then we don't add new features...
382  if (first_id < 0)
383  last_id = -1;
384  // Update features to match the ones in the given container
385  typename std::map<int, T>::iterator iter = container.begin();
386  typename std::map<int, T>::iterator iter_end = container.end();
387  feature_count = 0;
388  for (; iter != iter_end; iter++) {
389  T &f = iter->second;
390  if (!do_handle_test(f))
391  continue;
392  if (f.has_p2d != true)
393  continue;
394  f.has_p2d = false; // This is updated again to true if tracking succeeds
395  features[feature_count] = f.p2d;
396  ids[feature_count] = iter->first;
397  feature_count++;
398  if (feature_count == max_features)
399  break;
400  }
401  // Purge if needed
402  if (purge) {
404  purge = false;
405  }
406  if (first_id < 0) {
407  // Track as usual (this will swap above features to prev_features)
408  TrackHid(img, mask, false);
409  } else {
410  // Check that next_id is ok
411  if (next_id < first_id)
412  next_id = first_id;
413  if (next_id > last_id)
414  return false; // TODO: Make some better solution for this
415  // Track as usual (this will swap above features to prev_features)
416  TrackHid(img, mask, true);
417  }
418  // Update the container to have the updated features
419  for (int i = 0; i < feature_count; i++) {
420  int id = ids[i];
421  if (last_id >= 0 && id >= last_id)
422  break;
423  T &f = container[id];
424  f.type_id = type_id;
425  f.has_p2d = true;
426  f.p2d = features[i];
427  }
428  return true;
429  }
430 
432  template <typename T>
433  bool
434  AddFeatures(std::map<int, T> &container, int type_id = 0, int first_id = 0, int last_id = 65535)
435  {
436  if (TrackerFeatures::AddFeatures(mask = cv::Mat()) == 0)
437  return false;
438  // Update the container to have the updated features
439  for (int i = 0; i < feature_count; i++) {
440  int id = ids[i];
441  if (last_id >= 0 && id >= last_id)
442  break;
443  T &f = container[id];
444  f.type_id = type_id;
445  f.has_p2d = true;
446  f.p2d = features[i];
447  }
448  return true;
449  }
450 
452  template <typename T>
453  int
454  EraseNonTracked(std::map<int, T> &container, int type_id = -1)
455  {
456  DoEraseTest<T> do_erase_test(true, false, type_id);
457  return EraseItemsEC(container, do_erase_test);
458  }
463  void
465  {
466  purge = true;
467  }
468  void
469  Reset()
470  {
471  throw alvar::AlvarException("Method not supported");
472  }
473  double
474  Reset(cv::Mat &img, cv::Mat &mask)
475  {
476  throw alvar::AlvarException("Method not supported");
477  }
478  bool
479  DelFeature(int index)
480  {
481  throw alvar::AlvarException("Method not supported");
482  }
483  bool
484  DelFeatureId(int id)
485  {
486  throw alvar::AlvarException("Method not supported");
487  }
488 };
489 
491 class ALVAR_EXPORT CameraEC : public Camera
492 {
493 public:
495  template <typename T>
496  void
497  Undistort(std::map<int, T> &container, int type_id = -1)
498  {
499  DoHandleTest<T> do_handle_test_default(type_id);
500  return Undistort(container, do_handle_test_default);
501  }
503  template <typename T, typename F>
504  void
505  Undistort(std::map<int, T> &container, F &do_handle_test)
506  {
507  typename std::map<int, T>::iterator iter = container.begin();
508  typename std::map<int, T>::iterator iter_end = container.end();
509  for (; iter != iter_end; iter++) {
510  T &f = iter->second;
511  if (!do_handle(f))
512  continue;
513  if (f.has_p2d)
514  Undistort(f.p2d);
515  }
516  }
518  template <typename T>
519  void
520  Distort(std::map<int, T> &container, int type_id = -1)
521  {
522  DoHandleTest<T> do_handle_test_default(type_id);
523  return Distort(container, do_handle_test_default);
524  }
526  template <typename T, typename F>
527  void
528  Distort(std::map<int, T> &container, F &do_handle_test)
529  {
530  typename std::map<int, T>::iterator iter = container.begin();
531  typename std::map<int, T>::iterator iter_end = container.end();
532  for (; iter != iter_end; iter++) {
533  T &f = iter->second;
534  if (!do_handle(f))
535  continue;
536  if (f.has_p2d)
537  Distort(f.p2d);
538  }
539  }
541  template <typename T>
542  bool
543  CalcExteriorOrientation(std::map<int, T> &container, Pose *pose, int type_id = -1)
544  {
545  DoHandleTest<T> do_handle_test_default(type_id);
546  return CalcExteriorOrientation(container, pose, do_handle_test_default);
547  }
549  template <typename T, typename F>
550  bool
551  CalcExteriorOrientation(std::map<int, T> &container, Pose *pose, F do_handle_test)
552  {
553  int count_points = container.size();
554  if (count_points < 4)
555  return false;
556  cv::Mat world_points = cv::Mat(count_points, 1, CV_32FC3);
557  cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_32FC2);
558  typename std::map<int, T>::iterator iter = container.begin();
559  typename std::map<int, T>::iterator iter_end = container.end();
560  int ind = 0;
561  for (; iter != iter_end; iter++) {
562  T &f = iter->second;
563  if (!do_handle_test(f))
564  continue;
565  if (f.has_p2d && f.has_p3d) {
566  world_points.at<float>(ind * 3 + 0) = (float)f.p3d.x;
567  world_points.at<float>(ind * 3 + 1) = (float)f.p3d.y;
568  world_points.at<float>(ind * 3 + 2) = (float)f.p3d.z;
569  image_observations.at<float>(ind * 2 + 0) = (float)f.p2d.x;
570  image_observations.at<float>(ind * 2 + 1) = (float)f.p2d.y;
571  ind++;
572  }
573  }
574  if (ind < 4)
575  return false;
576 
577  cv::Rect r;
578  r.x = 0;
579  r.y = 0;
580  r.height = ind;
581  r.width = 1;
582  cv::Mat world_points_sub = world_points(r);
583  cv::Mat image_observations_sub = image_observations(r);
584 
585  bool ret = Camera::CalcExteriorOrientation(world_points_sub, image_observations_sub, pose);
586 
587  image_observations.release();
588  world_points.release();
589 
590  return ret;
591  }
593  template <typename T>
594  bool
595  UpdatePose(std::map<int, T> & container,
596  Pose * pose,
597  int type_id = -1,
598  std::map<int, double> *weights = 0)
599  {
600  DoHandleTest<T> do_handle_test_default(type_id);
601  return UpdatePose(container, pose, do_handle_test_default, weights);
602  }
604  template <typename T>
605  bool
606  UpdateRotation(std::map<int, T> &container, Pose *pose, int type_id = -1)
607  {
608  DoHandleTest<T> do_handle_test_default(type_id);
609  return UpdateRotation(container, pose, do_handle_test_default);
610  }
612  template <typename T, typename F>
613  bool
614  UpdateRotation(std::map<int, T> &container, Pose *pose, F do_handle_test)
615  {
616  int count_points = container.size();
617  if (count_points < 6)
618  return false;
619 
620  cv::Mat world_points = cv::Mat(count_points, 1, CV_64FC3);
621  cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_64F); // [u v u v u v ...]'
622 
623  int ind = 0;
624  typename std::map<int, T>::iterator iter = container.begin();
625  typename std::map<int, T>::iterator iter_end = container.end();
626  for (; iter != iter_end; iter++) {
627  T &f = iter->second;
628  if (!do_handle_test(f))
629  continue;
630  if (f.has_p2d && f.has_p3d) {
631  world_points.at<float>(ind * 3 + 0) = (float)f.p3d.x;
632  world_points.at<float>(ind * 3 + 1) = (float)f.p3d.y;
633  world_points.at<float>(ind * 3 + 2) = (float)f.p3d.z;
634  image_observations.at<float>(ind * 2 + 0) = (float)f.p2d.x;
635  image_observations.at<float>(ind * 2 + 1) = (float)f.p2d.y;
636  ind++;
637  }
638  }
639  if (ind < 6)
640  return false;
641 
642  cv::Rect r;
643  r.x = 0;
644  r.y = 0;
645  r.height = ind;
646  r.width = 1;
647  cv::Mat world_points_sub = world_points(r);
648 
649  r.height = 2 * ind;
650  cv::Mat image_observations_sub = image_observations(r);
651 
652  bool ret = UpdateRotation(world_points_sub, image_observations_sub, pose);
653 
654  image_observations.release();
655  world_points.release();
656 
657  return ret;
658  }
660  template <typename T, typename F>
661  bool
662  UpdatePose(std::map<int, T> & container,
663  Pose * pose,
664  F do_handle_test,
665  std::map<int, double> *weights = 0)
666  {
667  int count_points = container.size();
668  if (count_points < 4)
669  return false; // Note, UpdatePose calls CalcExteriorOrientation if 4 or 5 points
670 
671  cv::Mat world_points = cv::Mat(count_points, 1, CV_64FC3);
672  cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_64F); // [u v u v u v ...]'
673  cv::Mat w = cv::Mat();
674  if (weights)
675  w = cv::Mat(count_points * 2, 1, CV_64F);
676 
677  int ind = 0;
678  typename std::map<int, T>::iterator iter = container.begin();
679  typename std::map<int, T>::iterator iter_end = container.end();
680  for (; iter != iter_end; iter++) {
681  T &f = iter->second;
682  if (!do_handle_test(f))
683  continue;
684  if (f.has_p2d && f.has_p3d) {
685  world_points.at<float>(ind * 3 + 0) = (float)f.p3d.x;
686  world_points.at<float>(ind * 3 + 1) = (float)f.p3d.y;
687  world_points.at<float>(ind * 3 + 2) = (float)f.p3d.z;
688  image_observations.at<float>(ind * 2 + 0) = (float)f.p2d.x;
689  image_observations.at<float>(ind * 2 + 1) = (float)f.p2d.y;
690  if (weights)
691  w.at<float>(ind * 2 + 1) = w.at<float>(ind * 2 + 0) = (*weights)[iter->first];
692  ind++;
693  }
694  }
695  if (ind < 4)
696  return false; // Note, UpdatePose calls CalcExteriorOrientation if 4 or 5 points
697 
698  cv::Rect r;
699  r.x = 0;
700  r.y = 0;
701  r.height = ind;
702  r.width = 1;
703  cv::Mat world_points_sub = world_points(r);
704 
705  r.height = 2 * ind;
706  cv::Mat image_observations_sub = image_observations(r);
707 
708  bool ret;
709  if (weights) {
710  cv::Mat w_sub = w(r);
711  ret = UpdatePose(world_points_sub, image_observations_sub, pose, w_sub);
712  } else {
713  cv::Mat w_ones = cv::Mat::ones(w.size(), w.type());
714  ret = UpdatePose(
715  world_points_sub,
716  image_observations_sub,
717  pose,
718  w_ones); //weights equal to one should not temper with things if no weights were given? -Sebastian
719  }
720  image_observations.release();
721  world_points.release();
722  w.release();
723 
724  return ret;
725  }
726 
728  template <typename T>
729  float
730  Reproject(std::map<int, T> &container,
731  Pose * pose,
732  int type_id = -1,
733  bool needs_has_p2d = false,
734  bool needs_has_p3d = false,
735  float average_outlier_limit = 0.f)
736  {
737  DoHandleTest<T> do_handle_test(type_id, needs_has_p2d, needs_has_p3d);
738  return Reproject(container, pose, do_handle_test, average_outlier_limit);
739  }
741  template <typename T, typename F>
742  float
743  Reproject(std::map<int, T> &container,
744  Pose * pose,
745  F do_handle_test,
746  float average_outlier_limit = 0.f)
747  {
748  float reprojection_sum = 0.f;
749  int reprojection_count = 0;
750  float limit_sq = average_outlier_limit * average_outlier_limit;
751  typename std::map<int, T>::iterator iter = container.begin();
752  typename std::map<int, T>::iterator iter_end = container.end();
753  for (; iter != iter_end; iter++) {
754  T &f = iter->second;
755  if (!do_handle_test(f))
756  continue;
757  Camera::ProjectPoint(iter->second.p3d, pose, iter->second.projected_p2d);
758  Camera::ProjectPoint(iter->second.p3d_sh, pose, iter->second.projected_p2d_sh);
759 
760  // TODO: Now this is calculated in several places (should this distance be included in ExternalContainer structure?
761  float dist_sq = (f.p2d.x - f.projected_p2d.x) * (f.p2d.x - f.projected_p2d.x);
762  dist_sq += (f.p2d.y - f.projected_p2d.y) * (f.p2d.y - f.projected_p2d.y);
763  if ((limit_sq == 0) || (dist_sq < limit_sq)) {
764  reprojection_sum += sqrt(dist_sq);
765  reprojection_count++;
766  }
767  }
768  if (reprojection_count == 0)
769  return 0.f;
770  return reprojection_sum / reprojection_count;
771  }
772 
777  template <typename T>
778  int
779  EraseUsingReprojectionError(std::map<int, T> &container,
780  float reprojection_error_limit,
781  int type_id = -1,
782  Pose * pose = 0)
783  {
784  if (pose)
785  Reproject(container, pose, type_id, false, false);
786  DoEraseTest<T> do_erase_test(reprojection_error_limit, false, false, type_id);
787  return EraseItemsEC(container, do_erase_test);
788  }
789 
791  bool UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose);
792 
794  bool
795  UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rot, cv::Mat &tra);
796 
798  bool
799  UpdatePose(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose, cv::Mat &weights);
800 
802  bool UpdatePose(const cv::Mat &object_points,
803  cv::Mat & image_points,
804  cv::Mat & rodriques,
805  cv::Mat & tra,
806  cv::Mat & weights);
807 
809  bool ReconstructFeature(const Pose * pose1,
810  const Pose * pose2,
811  const cv::Point2f *u1,
812  const cv::Point2f *u2,
813  cv::Point3f * p3d,
814  double limit);
815 
817  void Get3dOnPlane(const Pose *pose, cv::Point2f p2d, cv::Point3f &p3d);
818 
820  void Get3dOnDepth(const Pose *pose, cv::Point2f p2d, float depth, cv::Point3f &p3d);
821 };
822 
824 inline int
825 MarkerIdToContainerId(int marker_id, int corner_id, int first_id = 0, int last_id = 65535)
826 {
827  int id = first_id + marker_id * 4 + corner_id;
828  if (id > last_id)
829  return -1;
830  return id;
831 }
832 
834 template <class M>
836 {
837 protected:
838  template <typename T>
839  bool
840  PreDetect(std::pair<const int, T> &p, int type_id)
841  {
842  return PreDetect(p.second, type_id);
843  }
844  template <typename T>
845  bool
846  PreDetect(T &p, int type_id)
847  {
848  if (p.type_id != type_id)
849  return false;
850  p.has_p2d = false; // This is updated again to true if tracking succeeds
851  return true;
852  }
853 
854 public:
855  int
856  GetId(int marker_id, int corner_id, int first_id = 0, int last_id = 65535)
857  {
858  return MarkerIdToContainerId(marker_id, corner_id, first_id, last_id);
859  }
860 
862  template <typename T>
863  int
864  Detect(cv::Mat & image,
865  Camera * cam,
866  std::map<int, T> &container,
867  int type_id = 0,
868  int first_id = 0,
869  int last_id = 65535,
870  bool track = false,
871  bool visualize = false,
872  double max_new_marker_error = 0.08,
873  double max_track_error = 0.2,
874  LabelingMethod labeling_method = CVSEQ)
875  {
876  int ret;
877 
878  // The existing markers in the container are marked to not have valid p2d (has_p2d=false)
879  typename std::map<int, T>::iterator iter = container.begin();
880  typename std::map<int, T>::iterator iter_end = container.end();
881  for (; iter != iter_end; iter++) {
882  T &f = iter->second;
883  if (f.type_id != type_id)
884  continue;
885  f.has_p2d = false; // This is updated again to true if tracking succeeds
886  }
887 
888  // Detect without making the unnecessary pose update
890  image, cam, track, visualize, max_new_marker_error, max_track_error, labeling_method, false);
891 
892  // Fill in the detected markers to the container
893  for (size_t i = 0; i < MarkerDetector<M>::markers->size(); i++) {
894  M &m = (*(MarkerDetector<M>::markers))[i];
895  for (int corner = 0; corner < 4; corner++) {
896  int id = MarkerIdToContainerId(m.GetId(), corner, first_id, last_id);
897  if (id != -1) {
898  T &f = container[id];
899  f.type_id = type_id;
900  f.has_p2d = true;
901  f.p2d.x = float(m.marker_corners_img[corner].x);
902  f.p2d.y = float(m.marker_corners_img[corner].y);
903  }
904  }
905  }
906  return ret;
907  }
908 
910  template <typename T>
911  void
912  MarkerToEC(std::map<int, T> &container,
913  int marker_id,
914  double edge_length,
915  Pose & pose,
916  int type_id = 0,
917  int first_id = 0,
918  int last_id = 65535)
919  {
920  cv::Mat m3 = cv::Mat(4, 4, CV_64F);
921  cv::setIdentity(m3);
922  pose.GetMatrix(m3);
923 
924  for (size_t corner = 0; corner < 4; ++corner) {
925  // TODO: This should be exactly the same as in Marker class.
926  // Should we get the values from there somehow?
927  double X_data[4] = {0, 0, 0, 1};
928  if (corner == 0) {
929  X_data[0] = -0.5 * edge_length;
930  X_data[1] = -0.5 * edge_length;
931  } else if (corner == 1) {
932  X_data[0] = +0.5 * edge_length;
933  X_data[1] = -0.5 * edge_length;
934  } else if (corner == 2) {
935  X_data[0] = +0.5 * edge_length;
936  X_data[1] = +0.5 * edge_length;
937  } else if (corner == 3) {
938  X_data[0] = -0.5 * edge_length;
939  X_data[1] = +0.5 * edge_length;
940  }
941 
942  cv::Mat X = cv::Mat(4, 1, CV_64F, X_data);
943  X = m3 * X;
944 
945  int id = MarkerIdToContainerId(marker_id, corner, first_id, last_id);
946  T & f = container[id];
947  f.type_id = type_id;
948  f.p3d.x = float(X_data[0] / X_data[3]);
949  f.p3d.y = float(X_data[1] / X_data[3]);
950  f.p3d.z = float(X_data[2] / X_data[3]);
951  f.has_p3d = true;
952  }
953  m3.release();
954  }
955 };
956 
959 {
960 public:
962  template <typename T>
963  bool
964  MarkersToEC(std::map<int, T> &container, int type_id = 0, int first_id = 0, int last_id = 65535)
965  {
966  bool ret = false;
967  for (size_t i = 0; i < marker_indices.size(); i++) {
968  if (marker_status[i] == 0)
969  continue; // Skip the ones not in point cloud
970  int marker_id = marker_indices[i];
971  for (int corner = 0; corner < 4; corner++) {
972  int id = MarkerIdToContainerId(marker_id, corner, first_id, last_id);
973  if (id != -1) {
974  int pc_index = pointcloud_index(marker_id, corner);
975  T & f = container[id];
976  f.type_id = type_id;
977  f.p3d.x = float(pointcloud[pc_index].x);
978  f.p3d.y = float(pointcloud[pc_index].y);
979  f.p3d.z = float(pointcloud[pc_index].z);
980  f.has_p3d = true;
981  ret = true;
982  }
983  }
984  }
985  return ret;
986  }
987 
988  template <typename T>
989  bool
990  MarkersFromEC(std::map<int, T> &container, int type_id = 0, int first_id = 0, int last_id = 65535)
991  {
992  // TODO...
993  return false;
994  }
995 
996  template <typename T>
997  bool
998  Save(std::map<int, T> &container,
999  const char * fname,
1001  int type_id = 0,
1002  int first_id = 0,
1003  int last_id = 65535)
1004  {
1005  if (!MarkersFromEC(container, type_id, first_id, last_id))
1006  return false;
1007  if (!MultiMarker::Save(fname, format))
1008  return false;
1009  return true;
1010  }
1011 
1013  template <typename T>
1014  bool
1015  Load(std::map<int, T> &container,
1016  const char * fname,
1018  int type_id = 0,
1019  int first_id = 0,
1020  int last_id = 65535)
1021  {
1022  if (!MultiMarker::Load(fname, format))
1023  return false;
1024  return MarkersToEC(container, type_id, first_id, last_id);
1025  }
1026 };
1027 } // namespace alvar
1028 
1029 #endif
This file implements a camera used for projecting points and computing homographies.
This file implements a generic marker detector.
This file implements a multi-marker.
This file implements a feature tracker.
ALVAR exception class.
Version of Camera using external container.
Definition: EC.h:492
bool UpdatePose(std::map< int, T > &container, Pose *pose, int type_id=-1, std::map< int, double > *weights=0)
Update the pose using the items with matching type_id.
Definition: EC.h:595
bool CalcExteriorOrientation(std::map< int, T > &container, Pose *pose, F do_handle_test)
Calculate the pose using the items matching the given functor.
Definition: EC.h:551
void Get3dOnDepth(const Pose *pose, cv::Point2f p2d, float depth, cv::Point3f &p3d)
Get 3D-coordinate for 2D feature assuming certain depth.
bool UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose)
Update pose rotations based on new observations.
void Undistort(std::map< int, T > &container, F &do_handle_test)
Undistort the items matching the given functor.
Definition: EC.h:505
bool UpdatePose(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose, cv::Mat &weights)
Update existing pose based on new observations.
void Undistort(std::map< int, T > &container, int type_id=-1)
Undistort the items with matching type_id.
Definition: EC.h:497
void Get3dOnPlane(const Pose *pose, cv::Point2f p2d, cv::Point3f &p3d)
Get 3D-coordinate for 2D feature on the plane defined by the pose (z == 0)
bool UpdateRotation(std::map< int, T > &container, Pose *pose, int type_id=-1)
Update the rotation in pose using the items with matching type_id.
Definition: EC.h:606
bool UpdatePose(std::map< int, T > &container, Pose *pose, F do_handle_test, std::map< int, double > *weights=0)
Update the pose using the items matching the given functor.
Definition: EC.h:662
bool CalcExteriorOrientation(std::map< int, T > &container, Pose *pose, int type_id=-1)
Calculate the pose using the items with matching type_id.
Definition: EC.h:543
int EraseUsingReprojectionError(std::map< int, T > &container, float reprojection_error_limit, int type_id=-1, Pose *pose=0)
Erases the items matching with type_id having large reprojection error. If type_id == -1 doesn't test...
Definition: EC.h:779
bool UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rot, cv::Mat &tra)
Update pose rotations (in rodriques&tra) based on new observations.
void Distort(std::map< int, T > &container, F &do_handle_test)
Distort the items matching the given functor.
Definition: EC.h:528
float Reproject(std::map< int, T > &container, Pose *pose, F do_handle_test, float average_outlier_limit=0.f)
Projects p3d in the items matching with the given functor.
Definition: EC.h:743
bool UpdatePose(const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rodriques, cv::Mat &tra, cv::Mat &weights)
Update existing pose (in rodriques&tra) based on new observations.
bool UpdateRotation(std::map< int, T > &container, Pose *pose, F do_handle_test)
Update the rotation in pose using the items matching the given functor.
Definition: EC.h:614
float Reproject(std::map< int, T > &container, Pose *pose, int type_id=-1, bool needs_has_p2d=false, bool needs_has_p3d=false, float average_outlier_limit=0.f)
Projects p3d in the items matching with type_id into projected_p2d . If type_id == -1 doesn't test th...
Definition: EC.h:730
void Distort(std::map< int, T > &container, int type_id=-1)
Distort the items with matching type_id.
Definition: EC.h:520
bool ReconstructFeature(const Pose *pose1, const Pose *pose2, const cv::Point2f *u1, const cv::Point2f *u2, cv::Point3f *p3d, double limit)
Reconstruct 3D point using two camera poses and corresponding undistorted image feature positions.
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
void ProjectPoint(const cv::Point3d &pw, const Pose *pose, cv::Point2d &pi) const
Project one point.
void CalcExteriorOrientation(const std::vector< cv::Point3d > &pw, const std::vector< PointDouble > &pi, cv::Mat &rodriques, cv::Mat &tra) const
Calculate exterior orientation.
This is default functor for testing which items in the container should be erased.
Definition: EC.h:220
This is a default functor for testing which items in the container should be handled by each method.
Definition: EC.h:188
Basic structure to be usable with EC methods.
Definition: EC.h:153
Version of MarkerDetector using external container.
Definition: EC.h:836
int Detect(cv::Mat &image, Camera *cam, std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535, bool track=false, bool visualize=false, double max_new_marker_error=0.08, double max_track_error=0.2, LabelingMethod labeling_method=CVSEQ)
Detect markers in the image and fill in their 2D-positions in the given external container.
Definition: EC.h:864
void MarkerToEC(std::map< int, T > &container, int marker_id, double edge_length, Pose &pose, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker_id marker corners using edge_length and pose.
Definition: EC.h:912
MarkerDetector for detecting markers of type M
int Detect(cv::Mat &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 's from image
Version of MultiMarker using external container.
Definition: EC.h:959
bool MarkersToEC(std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker corners to the given container.
Definition: EC.h:964
bool Load(std::map< int, T > &container, const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker corners to the given container using save MultiMarker file.
Definition: EC.h:1015
Base class for using MultiMarker.
Definition: MultiMarker.h:48
bool Save(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Saves multi marker configuration to a text file.
bool Load(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Loads multi marker configuration from a text file.
Pose representation derived from the Rotation class
Definition: Pose.h:53
void GetMatrix(cv::Mat &mat) const
Version of TrackerFeatures using external container.
Definition: EC.h:292
bool AddFeatures(std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535)
add features to the previously tracked frame if there are less than min_features
Definition: EC.h:434
bool Track(cv::Mat &img, cv::Mat &mask, std::map< int, T > &container, int type_id=-1, int first_id=-1, int last_id=-1)
Track features with matching type id. New features will have id's in the specified id range.
Definition: EC.h:317
TrackerFeaturesEC(int _max_features=100, int _min_features=90, double _quality_level=0.01, double _min_distance=10, int _pyr_levels=4, int win_size=6)
Constructor.
Definition: EC.h:298
void Purge()
Purge features that are considerably closer than the defined min_distance.
Definition: EC.h:464
int EraseNonTracked(std::map< int, T > &container, int type_id=-1)
Erases the items matching with type_id having has_p2d == false . If type_id == -1 doesn't test the ty...
Definition: EC.h:454
bool Track(cv::Mat &img, cv::Mat &mask, std::map< int, T > &container, F do_handle_test, int type_id=0, int first_id=-1, int last_id=-1)
Track features matching the given functor. New features will have id's in the specified id range.
Definition: EC.h:373
TrackerFeatures tracks features using OpenCV's cvGoodFeaturesToTrack and cvCalcOpticalFlowPyrLK
int Purge()
Purge features that are considerably closer than the defined min_distance.
int * ids
Track result: ID:s for current features
int feature_count
Track result: count of current features
int AddFeatures(cv::Mat &mask)
add features to the previously tracked frame if there are less than min_features
double TrackHid(const cv::Mat &img, cv::Mat &mask, bool add_features=true)
Reset track features on specified mask area.
std::vector< cv::Point2f > features
Track result: current features
Main ALVAR namespace.
Definition: Alvar.h:174
FILE_FORMAT
Definition: FileFormat.h:39
@ FILE_FORMAT_DEFAULT
Default file format.
Definition: FileFormat.h:45
int MarkerIdToContainerId(int marker_id, int corner_id, int first_id=0, int last_id=65535)
Calculate the index used in external container map for specified marker_id.
Definition: EC.h:825
int EraseItemsEC(std::map< int, T > &container, F do_erase_test)
Erasing items from container using DoEraseTest type functor.
Definition: EC.h:275