CaptureEngine.cpp
Go to the documentation of this file.
00001 /*
00002  * CaptureEngine.cpp
00003  *
00004  *  Created on: Oct 22, 2010
00005  *      Author: erublee
00006  */
00007 
00008 #include "pano_core/CaptureEngine.h"
00009 #include "pano_core/panoutils.h"
00010 
00011 #include <opencv2/highgui/highgui.hpp>
00012 #include<opencv2/legacy/legacy.hpp>
00013 
00014 using namespace cv;
00015 using namespace std;
00016 namespace pano
00017 {
00018 namespace
00019 {
00020 void onMatchCB(const AtomPair& pair)
00021 {
00022   static Mat matches_img;
00023   pair.atom2()->images().src().copyTo(matches_img);
00024   if (!pair.result().success())
00025     bitwise_not(matches_img, matches_img);
00026   drawMatchesRelative(pair.atom1()->features(), pair.atom2()->features(), pair.matches(), matches_img,
00027                       pair.result().inlier_mask());
00028   imshow("prior matches", matches_img);
00029 }
00030 }
00031 
00032 PriorTracker::PriorTracker(int max_fail_count) :
00033   max_fail_count_(max_fail_count), fail_count_(1)
00034 {
00035 
00036 }
00037 Extrinsics PriorTracker::track(ImageAtom& query, ModelFitter& fitter, FitterResult* result)
00038 {
00039 
00040   if (result != NULL)
00041   {
00042     *result = FitterResult();
00043   }
00044 
00045   if (prior_.features().kpts().empty())
00046   {
00047     prior_ = query;
00048     return query.extrinsics();
00049   }
00050   AtomPair pair = MoleculeProcessor::matchwithFitter(prior_.ptrToSelf(), query.ptrToSelf(), fitter);
00051   Extrinsics ext = pair.generateExtrinsics(query.ptrToSelf());
00052 
00053   callbacks_.callBack(pair, MATCHES_CALLBACK);
00054 
00055   if (pair.result().success())
00056   {
00057     if (angularDist(prior_.extrinsics(), query.extrinsics()) > CV_PI / 10)
00058     {
00059       prior_ = query;
00060       prior_.extrinsics() = ext;
00061     }
00062   }
00063   //  else
00064   //  {
00065   //    fail_count_++;
00066   //    if (fail_count_ > max_fail_count_)
00067   //    {
00068   //      prior_ = query;
00069   //    }
00070   //  }
00071 
00072   if (result != NULL)
00073   {
00074     *result = pair.result();
00075   }
00076 
00077   return ext;
00078 
00079 }
00080 void PriorTracker::updatePrior(const ImageAtom& prior)
00081 {
00082   prior_ = prior;
00083 }
00084 CaptureEngine::CaptureEngine(cv::Ptr<ModelFitter> fitter, cv::Ptr<cv::FeatureDetector> detector, Camera camera,
00085                              const std::string& dirname) :
00086   fitter_(fitter), detector_(detector), camera_(camera), atom_(camera, Images()), fail_count_(1), dirname_(dirname)
00087 {
00088   atom_.extrinsics() = Extrinsics(Mat::eye(3, 3, CV_32F), 1);
00089   atom_.extrinsics().flag(Extrinsics::ESTIMATED) = true;
00090   prior_tracker_.addMatchesCallback(&onMatchCB);
00091 }
00092 
00093 CaptureEngine::~CaptureEngine()
00094 {
00095 
00096 }
00097 
00098 cv::Ptr<ImageAtom> CaptureEngine::onNewFrame(const cv::Mat& frame)
00099 {
00100   if (!frame.empty())
00101   {
00102     // cout << "frame in capture engine: (W,H) = " << frame.cols << "," << frame.rows << endl;
00103   }
00104   else
00105   {
00106     cerr << "empty frame in capture engine! " << endl;
00107     return 0;
00108   }
00109   FitterResult fit_result;
00110   std::list<AtomPair> result;
00111   Ptr<ImageAtom> added_atom_really;
00112 
00113   atom_.images().load(frame);
00114   atom_.detect(*detector_);
00115   atom_.extract(BriefDescriptorExtractor(), BruteForceMatcher<Hamming> ());
00116   //tracker will return an estimated R by matching against a prior that it stores.
00117   atom_.extrinsics() = prior_tracker_.track(atom_, *fitter_, &fit_result);
00118   if (atom_.extrinsics().flag(Extrinsics::ESTIMATED))
00119   {
00120     //add the atom to the glob, its the first one
00121     if (glob_.getMolecules().empty())
00122     {
00123       glob_.addAtomToGlob(fitter_, atom_);
00124     }
00125     else
00126     {
00127       fail_count_ = 1;
00128       //find the closet atom in the glob
00129       Ptr<ImageAtom> min_atom = glob_.minDistAtom(atom_);
00130 
00131       //if the angle between the observation and the closest in the map is greater than some epsilon
00132       //try to add the observation to the map
00133       if (angularDist(atom_.extrinsics(), min_atom->extrinsics()) > atom_.camera().fov_max() / 4)
00134       {
00135         cout << "trying to add to map.." << endl;
00136         Ptr<ImageAtom> added_atom = glob_.queryAtomToGlob(fitter_, atom_, result);
00137         if (!result.empty())
00138         {
00139           cout << "added new image to map" << endl;
00140           glob_.addPrefittedPairs(result, added_atom);
00141           glob_.batchFindAndSetTrinsics();
00142           atom_.extrinsics() = added_atom->extrinsics();
00143           //update the prior to the image that was added, as this will be the best image
00144           prior_tracker_.updatePrior(*added_atom);
00145           added_atom_really = added_atom;
00146         }
00147 
00148       }
00149       else
00150       {
00151         //if the atom wasn't added then check the confidence level and update prior as necessary.
00152         //confidence should be roughly equivilant to an accumulated pixel error, big is worse
00153         if (atom_.extrinsics().val(Extrinsics::CONFIDENCE) > 20)
00154         {
00155           cout << "confidence " << atom_.extrinsics().val(Extrinsics::CONFIDENCE);//<<endl;
00156           cout << " switching out prior" << endl;
00157           prior_tracker_.updatePrior(*min_atom);
00158         }
00159         else
00160           prior_tracker_.updatePrior(atom_);
00161       }
00162 
00163     }
00164 
00165   }
00166   else
00167   {
00168     if (fail_count_++ % FAIL_MAX == 0)
00169     {
00170       cout << "lost localization! " << endl;
00171 
00172       //this function returns a pointer to an atom that might be added and fills out a list
00173       //of pairwise matches between matom and the glob atoms
00174       Ptr<ImageAtom> added_atom = glob_.queryAtomToGlob(fitter_, atom_, result);
00175 
00176       //this means that there are pairs that were successfully matched
00177       if (!result.empty())
00178       {
00179         cout << "localized" << endl;
00180         //if its new territory, add it to the map
00181         if (glob_.minDistToAtom(*added_atom) > atom_.camera().fov_max() / 4)
00182         {
00183           cout << "added new image to map" << endl;
00184           glob_.addPrefittedPairs(result);
00185           glob_.batchFindAndSetTrinsics();
00186           added_atom_really = added_atom;
00187         }
00188         atom_.extrinsics() = added_atom->extrinsics();
00189         prior_tracker_.updatePrior(*added_atom);
00190       }
00191     }
00192 
00193   }
00194   return added_atom_really;
00195 }
00196 
00197 void CaptureEngine::reset()
00198 {
00199   glob_.reset();
00200   atom_.extrinsics() = Extrinsics(Mat::eye(3, 3, CV_32F), 1);
00201   atom_.extrinsics().flag(Extrinsics::ESTIMATED) = true;
00202   prior_tracker_.updatePrior(atom_);
00203 }
00204 
00205 }


pano_core
Author(s): Ethan Rublee
autogenerated on Wed Aug 26 2015 16:34:01