EC.h
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 #ifndef EC_H
00025 #define EC_H
00026 
00034 #include "TrackerFeatures.h"
00035 #include "Camera.h"
00036 #include "MarkerDetector.h"
00037 #include "MultiMarker.h"
00038 
00039 namespace alvar {
00040 
00152 class ExternalContainer {
00153 public:
00154         int type_id;
00155         bool has_p2d;
00156         bool has_p3d;
00157         CvPoint2D32f p2d;
00158         CvPoint3D32f p3d;
00159         CvPoint2D32f projected_p2d; // This is only temporary -- user needs to take care that it has valid content
00160         ExternalContainer() : type_id(-1), has_p2d(false), has_p3d(false) {}
00161         ExternalContainer(const ExternalContainer &c) {
00162                 type_id = c.type_id;
00163                 has_p2d = c.has_p2d;
00164                 has_p3d = c.has_p3d;
00165                 p2d.x = c.p2d.x;
00166                 p2d.y = c.p2d.y;
00167                 p3d.x = c.p3d.x;
00168                 p3d.y = c.p3d.y;
00169                 p3d.z = c.p3d.z;
00170                 projected_p2d.x = c.projected_p2d.x;
00171                 projected_p2d.y = c.projected_p2d.y;
00172         }
00173 };
00174 
00181 template<typename T>
00182 class DoHandleTest {
00183 protected:
00184         int type_id;
00185         bool needs_has_p2d;
00186         bool needs_has_p3d;
00187 public:
00188         DoHandleTest(int _type_id=-1, bool _needs_has_p2d=false, bool _needs_has_p3d=false) 
00189                 : type_id(_type_id), needs_has_p2d(_needs_has_p2d), needs_has_p3d(_needs_has_p3d) {}
00190         virtual bool operator()(const T &f) const {
00191                 if (needs_has_p2d && !f.has_p2d) return false;
00192                 if (needs_has_p3d && !f.has_p3d) return false;
00193                 // if (f.type_id == -1) return false; // Unnecessary?
00194                 if ((type_id != -1) && (type_id != f.type_id)) return false;
00195                 return true;
00196         }
00197 };
00198 
00204 template<typename T>
00205 class DoEraseTest {
00206 protected:
00207         int type_id;
00208         bool erase_without_p2d;
00209         bool erase_without_p3d;
00210         bool test_reprojection;
00211         float limit_sq;
00212 public:
00213         DoEraseTest(bool _erase_without_p2d, bool _erase_without_p3d, int _type_id=-1) 
00214                 : type_id(_type_id), erase_without_p2d(_erase_without_p2d), erase_without_p3d(_erase_without_p3d), 
00215                   test_reprojection(false), limit_sq(0.f) {}
00216         DoEraseTest(float _limit, bool _erase_without_p2d=false, bool _erase_without_p3d=false, int _type_id=-1) 
00217                 : type_id(_type_id), erase_without_p2d(_erase_without_p2d), erase_without_p3d(_erase_without_p3d),
00218                   test_reprojection(true), limit_sq(_limit*_limit) {}
00219         virtual bool operator()(const T &f) const {
00220                 if ((type_id != -1) && (type_id != f.type_id)) return false;
00221                 if (erase_without_p2d && !f.has_p2d) return true;
00222                 if (erase_without_p3d && !f.has_p3d) return true;
00223                 if (test_reprojection) {
00224                         if (!f.has_p2d) return false;
00225                         if (!f.has_p3d) return false;
00226                         // Note that the projected_p2d needs to have valid content at this point
00227                         double dist_sq = (f.p2d.x-f.projected_p2d.x)*(f.p2d.x-f.projected_p2d.x);
00228                         dist_sq       += (f.p2d.y-f.projected_p2d.y)*(f.p2d.y-f.projected_p2d.y);
00229                         if (dist_sq > limit_sq) 
00230                                 return true;
00231                 }
00232                 return false;
00233         }
00234 };
00235 
00237 template<typename T, typename F>
00238 inline int EraseItemsEC(std::map<int,T> &container, F do_erase_test) {
00239         int count=0;
00240         typename std::map<int,T>::iterator iter = container.begin();
00241         while(iter != container.end()) {
00242                 T &f = iter->second;
00243                 if (do_erase_test(f)) {
00244                         container.erase(iter++);
00245                         count++;
00246                 } else iter++;
00247         }
00248         return count;
00249 }
00250 
00252 class TrackerFeaturesEC : public TrackerFeatures {
00253 protected:
00254         bool purge;
00255 public:
00257         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)
00258                 : TrackerFeatures(_max_features, _min_features, _quality_level, _min_distance, _pyr_levels, win_size), purge(false) {}
00259 
00261         template<typename T>
00262         bool Track(IplImage *img, IplImage *mask, std::map<int,T> &container, int type_id=-1, int first_id=-1, int last_id=-1)
00263         {
00264                 DoHandleTest<T> do_handle_test_default(type_id);
00265                 if (type_id == -1) type_id=0;
00266                 return Track(img, mask, container, do_handle_test_default, type_id, first_id, last_id);
00267         }
00268 
00270         /*template<typename T, typename F>
00271         bool Track(IplImage *img, IplImage *mask, std::map<int,T> &container, F do_handle_test, int type_id=0, int first_id=0, int last_id=65535)
00272         {
00273                 // Update features to match the ones in the given container
00274                 typename std::map<int,T>::iterator iter = container.begin();
00275                 typename std::map<int,T>::iterator iter_end = container.end();
00276                 feature_count = 0;
00277                 for (;iter != iter_end; iter++) {
00278                         T &f = iter->second;
00279                         if (!do_handle_test(f)) continue;
00280                         if (f.has_p2d != true) continue;
00281                         f.has_p2d = false; // This is updated again to true if tracking succeeds
00282                         features[feature_count] = f.p2d;
00283                         ids[feature_count] = iter->first;
00284                         feature_count++;
00285                         if (feature_count == max_features) break;
00286                 }
00287                 // Check that next_id is ok
00288                 if (next_id < first_id) next_id = first_id;
00289                 if (next_id > last_id) return false; // TODO: Make some better solution for this
00290                 // Purge if needed
00291                 if (purge) {
00292                         TrackerFeatures::Purge();
00293                         purge=false;
00294                 }
00295                 // Track as usual (this will swap above features to prev_features)
00296                 TrackHid(img, mask);
00297                 // Update the container to have the updated features
00298                 for (int i=0; i<feature_count; i++) {
00299                         int id = ids[i];
00300                         if (id >= last_id) break;
00301                         T &f = container[id];
00302                         f.type_id = type_id;
00303                         f.has_p2d = true;
00304                         f.p2d = features[i];
00305                 }
00306                 return true;
00307         }*/
00308 
00310         template<typename T, typename F>
00311         bool Track(IplImage *img, IplImage *mask, std::map<int,T> &container, F do_handle_test, int type_id=0, int first_id=-1, int last_id=-1)
00312         {
00313                 // When first_id && last_id are < 0 then we don't add new features...
00314                 if (first_id < 0) last_id = -1;
00315                 // Update features to match the ones in the given container
00316                 typename std::map<int,T>::iterator iter = container.begin();
00317                 typename std::map<int,T>::iterator iter_end = container.end();
00318                 feature_count = 0;
00319                 for (;iter != iter_end; iter++) {
00320                         T &f = iter->second;
00321                         if (!do_handle_test(f)) continue;
00322                         if (f.has_p2d != true) continue;
00323                         f.has_p2d = false; // This is updated again to true if tracking succeeds
00324                         features[feature_count] = f.p2d;
00325                         ids[feature_count] = iter->first;
00326                         feature_count++;
00327                         if (feature_count == max_features) break;
00328                 }
00329                 // Purge if needed
00330                 if (purge) {
00331                         TrackerFeatures::Purge();
00332                         purge=false;
00333                 }
00334                 if (first_id < 0) {
00335                         // Track as usual (this will swap above features to prev_features)
00336                         TrackHid(img, mask, false);
00337                 } else {
00338                         // Check that next_id is ok
00339                         if (next_id < first_id) next_id = first_id;
00340                         if (next_id > last_id) return false; // TODO: Make some better solution for this
00341                         // Track as usual (this will swap above features to prev_features)
00342                         TrackHid(img, mask, true);
00343                 }
00344                 // Update the container to have the updated features
00345                 for (int i=0; i<feature_count; i++) {
00346                         int id = ids[i];
00347                         if (last_id >= 0 && id >= last_id) break;
00348                         T &f = container[id];
00349                         f.type_id = type_id;
00350                         f.has_p2d = true;
00351                         f.p2d = features[i];
00352                 }
00353                 return true;
00354         }
00355 
00357         template<typename T>
00358         bool AddFeatures(std::map<int,T> &container, int type_id=0, int first_id=0, int last_id=65535)
00359         {
00360                 if (TrackerFeatures::AddFeatures() == 0) return false;
00361                 // Update the container to have the updated features
00362                 for (int i=0; i<feature_count; i++) {
00363                         int id = ids[i];
00364                         if (last_id >= 0 && id >= last_id) break;
00365                         T &f = container[id];
00366                         f.type_id = type_id;
00367                         f.has_p2d = true;
00368                         f.p2d = features[i];
00369                 }
00370                 return true;
00371         }
00372 
00374         template<typename T>
00375         int EraseNonTracked(std::map<int,T> &container, int type_id=-1)
00376         {
00377                 DoEraseTest<T> do_erase_test(true, false, type_id);
00378                 return EraseItemsEC(container, do_erase_test);
00379         }
00384         void Purge() { purge=true; }
00385         void Reset() { throw alvar::AlvarException("Method not supported"); }
00386         double Reset(IplImage *img, IplImage *mask) { throw alvar::AlvarException("Method not supported"); }
00387         bool DelFeature(int index) { throw alvar::AlvarException("Method not supported"); }
00388         bool DelFeatureId(int id) { throw alvar::AlvarException("Method not supported"); }
00389 };
00390 
00392 class ALVAR_EXPORT CameraEC : public Camera {
00393 public:
00395         template<typename T>
00396         void Undistort(std::map<int,T> &container, int type_id=-1) {
00397                 DoHandleTest<T> do_handle_test_default(type_id);
00398                 return Undistort(container, do_handle_test_default);
00399         }
00401         template<typename T, typename F>
00402         void Undistort(std::map<int,T> &container, F &do_handle_test) {
00403                 typename std::map<int,T>::iterator iter = container.begin();
00404                 typename std::map<int,T>::iterator iter_end = container.end();
00405                 for (;iter != iter_end; iter++) {
00406                         T &f = iter->second;
00407                         if (!do_handle(f)) continue;
00408                         if (f.has_p2d) Undistort(f.p2d);
00409                 }
00410         }
00412         template<typename T>
00413         void Distort(std::map<int,T> &container, int type_id=-1) {
00414                 DoHandleTest<T> do_handle_test_default(type_id);
00415                 return Distort(container, do_handle_test_default);
00416         }
00418         template<typename T, typename F>
00419         void Distort(std::map<int,T> &container, F &do_handle_test) {
00420                 typename std::map<int,T>::iterator iter = container.begin();
00421                 typename std::map<int,T>::iterator iter_end = container.end();
00422                 for (;iter != iter_end; iter++) {
00423                         T &f = iter->second;
00424                         if (!do_handle(f)) continue;
00425                         if (f.has_p2d) Distort(f.p2d);
00426                 }
00427         }
00429         template<typename T>
00430         bool CalcExteriorOrientation(std::map<int,T> &container, Pose *pose, int type_id=-1) {
00431                 DoHandleTest<T> do_handle_test_default(type_id);
00432                 return CalcExteriorOrientation(container, pose, do_handle_test_default);
00433         }
00435         template<typename T, typename F>
00436         bool CalcExteriorOrientation(std::map<int,T> &container, Pose *pose, F do_handle_test) {
00437                 int count_points = container.size();
00438                 if(count_points < 4) return false;
00439                 CvMat* world_points = cvCreateMat(count_points, 1, CV_32FC3);
00440                 CvMat* image_observations = cvCreateMat(count_points*2, 1, CV_32FC2);
00441                 typename std::map<int,T>::iterator iter = container.begin();
00442                 typename std::map<int,T>::iterator iter_end = container.end();
00443                 int ind = 0;
00444                 for (;iter != iter_end; iter++) {
00445                         T &f = iter->second;
00446                         if (!do_handle_test(f)) continue;
00447                         if (f.has_p2d && f.has_p3d) {
00448                                 world_points->data.fl[ind*3+0] = (float)f.p3d.x;
00449                                 world_points->data.fl[ind*3+1] = (float)f.p3d.y;
00450                                 world_points->data.fl[ind*3+2] = (float)f.p3d.z;
00451                                 image_observations->data.fl[ind*2+0]  = (float)f.p2d.x;
00452                                 image_observations->data.fl[ind*2+1]  = (float)f.p2d.y;
00453                                 ind++;
00454                         }
00455                 }
00456                 if (ind<4) return false;
00457 
00458                 CvRect r; r.x = 0; r.y = 0; r.height = ind; r.width = 1;
00459                 CvMat world_points_sub;
00460                 cvGetSubRect(world_points, &world_points_sub, r);
00461                 CvMat image_observations_sub;
00462                 cvGetSubRect(image_observations, &image_observations_sub, r);
00463 
00464                 bool ret = Camera::CalcExteriorOrientation(&world_points_sub, &image_observations_sub, pose);
00465 
00466                 cvReleaseMat(&image_observations);
00467                 cvReleaseMat(&world_points);
00468 
00469                 return ret;             
00470         }
00472         template<typename T>
00473         bool UpdatePose(std::map<int,T> &container, Pose *pose, int type_id=-1, std::map<int,double> *weights=0) {
00474                 DoHandleTest<T> do_handle_test_default(type_id);
00475                 return UpdatePose(container, pose, do_handle_test_default, weights);
00476         }
00478         template<typename T>
00479         bool UpdateRotation(std::map<int,T> &container, Pose *pose, int type_id=-1) {
00480                 DoHandleTest<T> do_handle_test_default(type_id);
00481                 return UpdateRotation(container, pose, do_handle_test_default);
00482         }
00484         template<typename T, typename F>
00485         bool UpdateRotation(std::map<int,T> &container, Pose *pose, F do_handle_test) {
00486                 int count_points = container.size();
00487                 if(count_points < 6) return false;
00488 
00489                 CvMat* world_points = cvCreateMat(count_points, 1, CV_64FC3);
00490                 CvMat* image_observations = cvCreateMat(count_points*2, 1, CV_64F); // [u v u v u v ...]'
00491 
00492                 int ind = 0;
00493                 typename std::map<int,T>::iterator iter = container.begin();
00494                 typename std::map<int,T>::iterator iter_end = container.end();
00495                 for (;iter != iter_end; iter++) {
00496                         T &f = iter->second;
00497                         if (!do_handle_test(f)) continue;
00498                         if (f.has_p2d && f.has_p3d) {
00499                                 world_points->data.db[ind*3+0] = f.p3d.x;
00500                                 world_points->data.db[ind*3+1] = f.p3d.y;
00501                                 world_points->data.db[ind*3+2] = f.p3d.z;
00502                                 image_observations->data.db[ind*2+0] = f.p2d.x;
00503                                 image_observations->data.db[ind*2+1] = f.p2d.y;
00504                                 ind++;
00505                         }
00506                 }
00507                 if (ind < 6) return false;
00508 
00509                 CvRect r; r.x = 0; r.y = 0; r.height = ind; r.width = 1;
00510                 CvMat world_points_sub;
00511                 cvGetSubRect(world_points, &world_points_sub, r);
00512 
00513                 r.height = 2*ind;
00514                 CvMat image_observations_sub;
00515                 cvGetSubRect(image_observations, &image_observations_sub, r);
00516 
00517                 bool ret = UpdateRotation(&world_points_sub, &image_observations_sub, pose);
00518 
00519                 cvReleaseMat(&image_observations);
00520                 cvReleaseMat(&world_points);
00521 
00522                 return ret;             
00523         }       
00525         template<typename T, typename F>
00526         bool UpdatePose(std::map<int,T> &container, Pose *pose, F do_handle_test, std::map<int,double> *weights=0) {
00527                 int count_points = container.size();
00528                 if(count_points < 4) return false; // Note, UpdatePose calls CalcExteriorOrientation if 4 or 5 points
00529 
00530                 CvMat* world_points = cvCreateMat(count_points, 1, CV_64FC3);
00531                 CvMat* image_observations = cvCreateMat(count_points*2, 1, CV_64F); // [u v u v u v ...]'
00532                 CvMat* w = 0;
00533                 if(weights) w = cvCreateMat(count_points*2, 1, CV_64F);
00534 
00535                 int ind = 0;
00536                 typename std::map<int,T>::iterator iter = container.begin();
00537                 typename std::map<int,T>::iterator iter_end = container.end();
00538                 for (;iter != iter_end; iter++) {
00539                         T &f = iter->second;
00540                         if (!do_handle_test(f)) continue;
00541                         if (f.has_p2d && f.has_p3d) {
00542                                 world_points->data.db[ind*3+0] = f.p3d.x;
00543                                 world_points->data.db[ind*3+1] = f.p3d.y;
00544                                 world_points->data.db[ind*3+2] = f.p3d.z;
00545                                 image_observations->data.db[ind*2+0] = f.p2d.x;
00546                                 image_observations->data.db[ind*2+1] = f.p2d.y;
00547 
00548                                 if(weights) 
00549                                         w->data.db[ind*2+1] = w->data.db[ind*2+0] = (*weights)[iter->first];
00550                                 ind++;
00551                         }
00552                 }
00553                 if (ind < 4) return false; // Note, UpdatePose calls CalcExteriorOrientation if 4 or 5 points
00554 
00555                 CvRect r; r.x = 0; r.y = 0; r.height = ind; r.width = 1;
00556                 CvMat world_points_sub;
00557                 cvGetSubRect(world_points, &world_points_sub, r);
00558 
00559                 r.height = 2*ind;
00560                 CvMat image_observations_sub;
00561                 cvGetSubRect(image_observations, &image_observations_sub, r);
00562 
00563                 bool ret;
00564                 if (weights) {
00565                         CvMat w_sub;
00566                         cvGetSubRect(w, &w_sub, r);
00567                         ret = UpdatePose(&world_points_sub, &image_observations_sub, pose, &w_sub);
00568                 }
00569                 else 
00570                         ret = UpdatePose(&world_points_sub, &image_observations_sub, pose);
00571 
00572                 cvReleaseMat(&image_observations);
00573                 cvReleaseMat(&world_points);
00574                 if(w) cvReleaseMat(&w);
00575 
00576                 return ret;             
00577         }
00578 
00580         template<typename T>
00581         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)
00582         {
00583                 DoHandleTest<T> do_handle_test(type_id, needs_has_p2d, needs_has_p3d);
00584                 return Reproject(container, pose, do_handle_test, average_outlier_limit);
00585         }
00587         template<typename T, typename F>
00588         float Reproject(std::map<int,T> &container, Pose *pose, F do_handle_test, float average_outlier_limit=0.f)
00589         {
00590                 float reprojection_sum=0.f;
00591                 int   reprojection_count=0;
00592                 float limit_sq = average_outlier_limit*average_outlier_limit;
00593                 typename std::map<int,T>::iterator iter = container.begin();
00594                 typename std::map<int,T>::iterator iter_end = container.end();
00595                 for(;iter != iter_end;iter++) {
00596                         T &f = iter->second;
00597                         if (!do_handle_test(f)) continue;
00598                         Camera::ProjectPoint(iter->second.p3d, pose, iter->second.projected_p2d);
00599                         Camera::ProjectPoint(iter->second.p3d_sh, pose, iter->second.projected_p2d_sh);
00600 
00601                         // TODO: Now this is calculated in several places (should this distance be included in ExternalContainer structure?
00602                         float dist_sq = (f.p2d.x-f.projected_p2d.x)*(f.p2d.x-f.projected_p2d.x);
00603                         dist_sq       += (f.p2d.y-f.projected_p2d.y)*(f.p2d.y-f.projected_p2d.y);
00604                         if ((limit_sq == 0) || (dist_sq < limit_sq)) {
00605                                 reprojection_sum += sqrt(dist_sq);
00606                                 reprojection_count++;
00607                         }
00608                 }
00609                 if (reprojection_count == 0) return 0.f;
00610                 return reprojection_sum/reprojection_count;
00611         }
00612 
00617         template<typename T>
00618         int EraseUsingReprojectionError(std::map<int,T> &container, float reprojection_error_limit, int type_id=-1, Pose *pose = 0)
00619         {
00620                 if (pose) Reproject(container, pose, type_id, false, false);
00621                 DoEraseTest<T> do_erase_test(reprojection_error_limit, false, false, type_id) ;
00622                 return EraseItemsEC(container, do_erase_test);
00623         }
00624 
00626         bool UpdateRotation(const CvMat* object_points, CvMat* image_points, Pose *pose);
00627 
00629         bool UpdateRotation(const CvMat* object_points, CvMat* image_points, CvMat *rot, CvMat *tra);
00630 
00632         bool UpdatePose(const CvMat* object_points, CvMat* image_points, Pose *pose, CvMat *weights=0);
00633 
00635         bool UpdatePose(const CvMat* object_points, CvMat* image_points, CvMat *rodriques, CvMat *tra, CvMat *weights=0);
00636 
00638         bool ReconstructFeature(const Pose *pose1, const Pose *pose2, const CvPoint2D32f *u1, const CvPoint2D32f *u2, CvPoint3D32f *p3d, double limit);
00639 
00641         void Get3dOnPlane(const Pose *pose, CvPoint2D32f p2d, CvPoint3D32f &p3d);
00642 
00644         void Get3dOnDepth(const Pose *pose, CvPoint2D32f p2d, float depth, CvPoint3D32f &p3d);
00645 };
00646 
00648 inline int MarkerIdToContainerId(int marker_id, int corner_id, int first_id=0, int last_id=65535) {
00649         int id = first_id + marker_id*4 + corner_id;
00650         if (id > last_id) return -1;
00651         return id;
00652 }
00653 
00655 template<class M>
00656 class MarkerDetectorEC : public MarkerDetector<M> {
00657 protected:
00658         template<typename T>
00659         bool PreDetect(std::pair<const int,T> &p, int type_id) {
00660                 return PreDetect(p.second, type_id);
00661         }
00662         template<typename T>
00663         bool PreDetect(T &p, int type_id) {
00664                 if (p.type_id != type_id) return false;
00665                 p.has_p2d = false; // This is updated again to true if tracking succeeds
00666                 return true;
00667         }
00668 public:
00669         int GetId(int marker_id, int corner_id, int first_id=0, int last_id=65535) {
00670                 return MarkerIdToContainerId(marker_id, corner_id, first_id, last_id);
00671         }
00672 
00674         template<typename T>
00675         int Detect(IplImage *image,
00676                            Camera *cam,
00677                            std::map<int,T> &container,
00678                            int type_id=0,
00679                            int first_id=0,
00680                            int last_id=65535,
00681                            bool track=false,
00682                            bool visualize=false,
00683                            double max_new_marker_error=0.08,
00684                            double max_track_error=0.2,
00685                            LabelingMethod labeling_method=CVSEQ)
00686         {
00687                 int ret;
00688 
00689                 // The existing markers in the container are marked to not have valid p2d (has_p2d=false)
00690                 typename std::map<int,T>::iterator iter = container.begin();
00691                 typename std::map<int,T>::iterator iter_end = container.end();
00692                 for (;iter != iter_end; iter++) {
00693                         T &f = iter->second;
00694                         if (f.type_id != type_id) continue;
00695                         f.has_p2d = false; // This is updated again to true if tracking succeeds
00696                 }
00697 
00698                 // Detect without making the unnecessary pose update
00699                 ret = MarkerDetector<M>::Detect(image, cam, track, visualize, max_new_marker_error, max_track_error, labeling_method, false);
00700 
00701                 // Fill in the detected markers to the container
00702                 for (size_t i=0; i<MarkerDetector<M>::markers->size(); i++) {
00703                         M &m = (*(MarkerDetector<M>::markers))[i];
00704                         for (int corner=0; corner<4; corner++) {
00705                                 int id = MarkerIdToContainerId(m.GetId(), corner, first_id, last_id);
00706                                 if (id != -1) {
00707                                         T &f = container[id];
00708                                         f.type_id = type_id;
00709                                         f.has_p2d = true;
00710                                         f.p2d.x = float(m.marker_corners_img[corner].x);
00711                                         f.p2d.y = float(m.marker_corners_img[corner].y);
00712                                 }
00713                         }
00714                 }
00715                 return ret;
00716         }
00717 
00719         template<typename T>
00720         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) {
00721                 CvMat *m3 = cvCreateMat(4,4,CV_64F); cvSetIdentity(m3);
00722                 pose.GetMatrix(m3);
00723 
00724                 for(size_t corner = 0; corner < 4; ++corner)
00725                 {
00726                         // TODO: This should be exactly the same as in Marker class.
00727                         //       Should we get the values from there somehow?
00728                         double X_data[4] = {0, 0, 0, 1};
00729                         if (corner == 0) { 
00730                                 X_data[0] = -0.5*edge_length;
00731                                 X_data[1] = -0.5*edge_length;
00732                         } else if (corner == 1) {
00733                                 X_data[0] = +0.5*edge_length;
00734                                 X_data[1] = -0.5*edge_length;
00735                         } else if (corner == 2) {
00736                                 X_data[0] = +0.5*edge_length;
00737                                 X_data[1] = +0.5*edge_length;
00738                         } else if (corner == 3) {
00739                                 X_data[0] = -0.5*edge_length;
00740                                 X_data[1] = +0.5*edge_length;
00741                         }
00742 
00743                         CvMat X  = cvMat(4, 1, CV_64F, X_data);
00744                         cvMatMul(m3, &X, &X);
00745 
00746                         int id = MarkerIdToContainerId(marker_id, corner, first_id, last_id);
00747                         T &f = container[id];
00748                         f.type_id = type_id;
00749                         f.p3d.x = float(X_data[0] / X_data[3]);
00750                         f.p3d.y = float(X_data[1] / X_data[3]);
00751                         f.p3d.z = float(X_data[2] / X_data[3]);
00752                         f.has_p3d = true;
00753                 }
00754                 cvReleaseMat(&m3);
00755         }
00756 };
00757 
00759 class MultiMarkerEC : public MultiMarker {
00760 public:
00762         template<typename T>
00763         bool MarkersToEC(std::map<int,T> &container, int type_id=0, int first_id=0,int last_id=65535) {
00764                 bool ret = false;
00765                 for (size_t i = 0; i < marker_indices.size(); i++) {
00766                         if (marker_status[i] == 0) continue; // Skip the ones not in point cloud 
00767                         int marker_id = marker_indices[i];
00768                         for (int corner = 0; corner < 4; corner++) {
00769                                 int id = MarkerIdToContainerId(marker_id, corner, first_id, last_id);
00770                                 if (id != -1) {
00771                                         int pc_index =  pointcloud_index(marker_id, corner);
00772                                         T &f = container[id];
00773                                         f.type_id = type_id;
00774                                         f.p3d.x = float(pointcloud[pc_index].x);
00775                                         f.p3d.y = float(pointcloud[pc_index].y);
00776                                         f.p3d.z = float(pointcloud[pc_index].z);
00777                                         f.has_p3d = true;
00778                                         ret = true;
00779                                 }
00780                         }
00781                 }
00782                 return ret;
00783         }
00784 
00785         template<typename T>
00786         bool MarkersFromEC(std::map<int,T> &container, int type_id=0, int first_id=0,int last_id=65535) {
00787                 // TODO...
00788                 return false;
00789         }
00790 
00791         template<typename T>
00792         bool Save(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) {
00793                 if (!MarkersFromEC(container, type_id, first_id, last_id)) return false;
00794                 if (!MultiMarker::Save(fname, format)) return false;
00795                 return true;
00796         }
00797 
00799         template<typename T>
00800         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) {
00801                 if (!MultiMarker::Load(fname, format)) return false;
00802                 return MarkersToEC(container, type_id, first_id, last_id);
00803         }
00804 };
00805 }
00806 
00807 #endif
00808 


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