MoleculeProcessor.cpp
Go to the documentation of this file.
00001 /*
00002  * MoleculeProcessor.cpp
00003  *
00004  *  Created on: Oct 20, 2010
00005  *      Author: erublee
00006  */
00007 
00008 #include "pano_core/MoleculeProcessor.h"
00009 #include "pano_core/ModelFitter.h"
00010 #include "pano_core/ImageAtom.h"
00011 #include "pano_core/PairNode.h"
00012 #include "pano_core/panoutils.h"
00013 #include "pano_core/feature_utils.h"
00014 
00015 #include <iostream>
00016 #include <algorithm>
00017 #include <iomanip>
00018 #include <map>
00019 
00020 using namespace cv;
00021 using namespace std;
00022 namespace pano
00023 {
00024 
00025 const std::string MoleculeProcessor::SHOW_PAIRS = "SHOW_PAIRS";
00026 const std::string MoleculeProcessor::GRAPHVIZ_FILENAME = "pairgraph.viz";
00027 const std::string MoleculeProcessor::VERBOSE_GRAPHVIZ_FILENAME = "pairgraph.verbose.viz";
00028 
00029 MoleculeProcessor::MoleculeProcessor()
00030 {
00031 }
00032 
00033 AtomPair MoleculeProcessor::matchwithFitter(Ptr<ImageAtom> prior, Ptr<ImageAtom> query, ModelFitter& modelfitter)
00034 {
00035   CV_Assert(query != prior)
00036     ;
00037 
00038   std::vector<DMatch> matches;
00039   prior->match(*query, matches, Mat(), 10);
00040   AtomPair pair(prior, query, matches);
00041   modelfitter.fit(pair);
00042   return pair;
00043 }
00044 
00045 std::list<AtomPair> MoleculeProcessor::queryWithAtom(const ImageMolecule& molecule, Ptr<ImageAtom> query, Ptr<
00046     ModelFitter> modelfitter, float angle_thresh)
00047 {
00048   float min_err = 10.0e8;
00049   std::list<AtomPair> pairsFound;
00050   if (modelfitter.empty())
00051   {
00052     cerr << "no fitter passed!" << endl;
00053     return pairsFound;
00054   }
00055   const set<Ptr<ImageAtom> >& atoms = molecule.getAtoms();
00056   if (0 == atoms.size())
00057   { // return empty list if no pairs to make
00058     cerr << "no atoms" << endl;
00059     return pairsFound;
00060   }
00061   if (atoms.count(query) > 0)
00062   { //handle if the atom is here already
00063     cerr << "ImageMolecule: already contains the atom passed! \n";
00064     return pairsFound;
00065   }
00066   else
00067   {
00068     Extrinsics& est_ext = query->extrinsics();
00069     set<Ptr<ImageAtom> >::const_iterator ait = atoms.begin();
00070     for (; ait != atoms.end(); ++ait)
00071     {
00072       Ptr<ImageAtom> prior = *ait;
00073       // first: if atom1 and atom2 both have trinsics set,
00074       // are they within the valid range of angle_thresh?
00075       if (angle_thresh > 0.0 && prior->extrinsics().flag(Extrinsics::ESTIMATED) && est_ext.flag(Extrinsics::ESTIMATED))
00076       {
00077         bool bAreCloseEnough = angularDist(prior->extrinsics(), est_ext) < angle_thresh;
00078         if (!bAreCloseEnough)
00079         {
00080           //cout << " atoms not close enough... " << endl;
00081           continue;
00082         }
00083       }
00084 
00085       AtomPair atompair = matchwithFitter(prior, query, modelfitter);
00086 
00087       if (atompair.result().success())
00088       {
00089 
00090         if (min_err > atompair.result().err())
00091         {
00092           min_err = atompair.result().err();
00093           est_ext = atompair.generateExtrinsics(query);
00094           if (angle_thresh > 0)
00095           {
00096             angle_thresh *= 0.7;
00097           }
00098           else
00099             angle_thresh = query->camera().fov_max();
00100         }
00101         pairsFound.push_back(atompair);
00102       }
00103     }
00104   }
00105   return pairsFound;
00106 }
00107 
00108 void DijkstraWay(ImageMolecule& mol)
00109 {
00110 
00111   if (mol.getAtoms().size() < 2)
00112     return;
00113   map<Ptr<ImageAtom> , PairNode> dijkstra_node_map;
00114   PairNodeData::max_depth = 0;
00115   PairNodeData::min_depth = 0;
00116 
00117   Ptr<ImageAtom> current = mol.getAnchor();
00118   dijkstra_node_map[current].node_data.distance = 0;
00119   if (current)
00120   {
00121     Extrinsics& anchorExt = current->extrinsics();
00122     anchorExt.val(Extrinsics::CONFIDENCE) = dijkstra_node_map[current].bconf_func(1e-1, 1e-1);
00123   }
00124   set<Ptr<ImageAtom> > Q = mol.getAtoms();
00125 
00126   while (!Q.empty() && !current.empty())
00127   {
00128     //  cout << "querying atom with UUID:  " << current->uid() << endl;
00129     const list<int>* pairs = mol.getPairIndices(current);
00130     PairNode& node = dijkstra_node_map[current];
00131     node.atom = current;
00132     for (list<int>::const_iterator it = pairs->begin(); it != pairs->end(); ++it)
00133     {
00134       const AtomPair* pair = mol.getPairByIndex(*it);
00135       PairNode& nextnode = dijkstra_node_map[pair->other(current)];
00136       nextnode.atom = pair->other(current);
00137       if (!nextnode.node_data.visited)
00138       {
00139         nextnode.setDist(*pair, node);
00140       }
00141 
00142     }
00143     float minnext = INFINITY;
00144     node.node_data.visited = true;
00145     Q.erase(current);
00146     current = NULL;
00147     for (set<Ptr<ImageAtom> >::iterator it = Q.begin(); it != Q.end(); ++it)
00148     {
00149       if (minnext > dijkstra_node_map[*it].node_data.distance)
00150       {
00151         minnext = dijkstra_node_map[*it].node_data.distance;
00152         current = *it;
00153         const std::list<int>* cur_pairs = mol.getPairIndices(current);
00154         for (std::list<int>::const_iterator pair_idx = cur_pairs->begin(); pair_idx != cur_pairs->end(); ++pair_idx)
00155         {
00156           dijkstra_node_map[current].node_data.neighbors.insert(*(mol.getPairByIndex(*pair_idx)));
00157         }
00158       }
00159     }
00160   }
00161 
00162 #if 1
00163   // no graphviz on phone!
00164 #else
00165   // gv file 1: the path used to set trinsics
00166   std::stringstream ss;
00167   ss << MoleculeProcessor::GRAPHVIZ_FILENAME.c_str();
00168   std::ofstream out(ss.str().c_str());
00169   PairNode::graphviz_dump(out, dijkstra_node_map);
00170 
00171   // gv file 2: verbose file, with all 'implicit errors' between pairs
00172   std::stringstream ss_verbose;
00173   ss_verbose << MoleculeProcessor::VERBOSE_GRAPHVIZ_FILENAME.c_str();
00174   std::ofstream vout(ss_verbose.str().c_str());
00175   PairNode::graphviz_dump_all(vout, dijkstra_node_map);
00176 #endif
00177 
00178 }
00179 
00180 void MoleculeProcessor::findAndSetTrinsics(ImageMolecule& mol, TFinder WAY)
00181 {
00182 
00183   switch (WAY)
00184   {
00185     case DIJKSTRA:
00186       DijkstraWay(mol);
00187       break;
00188   }
00189 
00190 }
00191 MoleculeGlob::MoleculeGlob() :
00192   uid_count_(0)
00193 {
00194 
00195 }
00196 float MoleculeGlob::minDistToAtom(const ImageAtom& atom) const
00197 {
00198 
00199   Ptr<ImageAtom> minAtom = minDistAtom(atom);
00200   if (!minAtom.empty())
00201     return angularDist(atom.extrinsics(), minAtom->extrinsics());
00202   return CV_PI;
00203 }
00204 cv::Ptr<ImageAtom> MoleculeGlob::minDistAtom(const ImageAtom& atom) const
00205 {
00206   float min_dist = CV_PI * 2;
00207 
00208   if (!atom.extrinsics().flag(Extrinsics::ESTIMATED))
00209     return NULL;
00210 
00211   Ptr<ImageAtom> min_atom;
00212 
00213   std::set<Ptr<ImageMolecule> >::const_iterator m_iter = molecules.begin();
00214 
00215   for (; m_iter != molecules.end(); ++m_iter)
00216   {
00217     const set<Ptr<ImageAtom> >& atoms = (*m_iter)->getAtoms();
00218     set<Ptr<ImageAtom> >::const_iterator ait = atoms.begin();
00219 
00220     for (; ait != atoms.end(); ++ait)
00221     {
00222       const Ptr<ImageAtom>& prior = *ait;
00223 
00224       float n_min = angularDist(prior->extrinsics(), atom.extrinsics());
00225       if (n_min < min_dist)
00226       {
00227         min_dist = n_min;
00228         min_atom = prior;
00229       }
00230     }
00231   }
00232   return min_atom;
00233 }
00234 namespace
00235 {
00236 
00237 struct QueryIdx
00238 {
00239   inline int operator()(const DMatch& m) const
00240   {
00241     return m.queryIdx;
00242   }
00243 };
00244 
00245 struct TrainIdx
00246 {
00247   inline int operator()(const DMatch& m) const
00248   {
00249     return m.trainIdx;
00250   }
00251 };
00252 
00253 template<class Idx>
00254   struct CompareOpIdx
00255   {
00256     Idx idx;
00257     inline bool operator()(const DMatch& lhs, const DMatch& rhs) const
00258     {
00259       return idx(lhs) < idx(rhs) || (idx(lhs) == idx(rhs) && lhs.distance < rhs.distance);
00260     }
00261 
00262   };
00263 
00264 template<class Idx>
00265   struct EqualOpIdx
00266   {
00267     Idx idx;
00268     inline bool operator()(const DMatch& lhs, const DMatch& rhs) const
00269     {
00270       return idx(lhs) == idx(rhs);// || (idx(lhs) == idx(rhs) && lhs.distance < rhs.distance);
00271     }
00272 
00273   };
00274 
00275 template<class IdxOp>
00276   inline void uniqueMatches(const MatchesVector& matches, MatchesVector& output, const IdxOp& idx)
00277   {
00278     output = matches;
00279     std::sort(output.begin(), output.end(), CompareOpIdx<IdxOp> ());
00280     std::unique(output.begin(), output.end(), EqualOpIdx<IdxOp> ());
00281   }
00282 
00283 void uniqueMatches(const MatchesVector& matches, MatchesVector& output)
00284 {
00285   uniqueMatches(matches, output, QueryIdx());
00286   uniqueMatches(output, output, TrainIdx());
00287   std::sort(output.begin(),output.end());
00288 }
00289 
00290 void flattenKNN(const vector<vector<DMatch> >& matches, vector<DMatch>& out)
00291 {
00292   out.clear();
00293   size_t count = 0;
00294   for (size_t i = 0; i < matches.size(); i++)
00295   {
00296     count += matches[i].size();
00297   }
00298   out.reserve(count);
00299   for (size_t i = 0; i < matches.size(); i++)
00300   {
00301     out.insert(out.end(), matches[i].begin(), matches[i].end());
00302   }
00303 
00304 }
00305 void maskMatchesByTrainImgIdx(const vector<DMatch>& matches, int trainImgIdx, vector<char>& mask)
00306 {
00307   mask.resize(matches.size());
00308   fill(mask.begin(), mask.end(), 0);
00309   for (size_t i = 0; i < matches.size(); i++)
00310   {
00311     if (matches[i].imgIdx == trainImgIdx)
00312       mask[i] = 1;
00313   }
00314 }
00315 
00316 void condenseByTrainImgIdx(vector<DMatch>& matches, int trainImgIdx, vector<DMatch>& tmatches)
00317 {
00318   tmatches.clear();
00319   for (size_t i = 0; i < matches.size(); i++)
00320   {
00321     //distance threshold is from experimentation
00322     if (matches[i].imgIdx == trainImgIdx)
00323     {
00324       //if (matches[i].distance < 90)
00325       tmatches.push_back(matches[i]);
00326       matches[i] = matches.back();
00327       matches.pop_back();
00328       i--;
00329     }
00330   }
00331   uniqueMatches(tmatches, tmatches);
00332 }
00333 struct ptrReplace
00334 {
00335   Ptr<ImageAtom> query;
00336   ptrReplace(Ptr<ImageAtom> query) :
00337     query(query)
00338   {
00339   }
00340   void operator()(AtomPair& apair)
00341   {
00342     apair.setAtom2(query);
00343   }
00344 };
00345 }
00346 Ptr<ImageAtom> MoleculeGlob::queryAtomToGlob(cv::Ptr<ModelFitter> fitter, const ImageAtom& _atom,
00347                                              std::list<AtomPair>& pairs, bool clone)
00348 {
00349 
00350   Ptr<ImageAtom> query;// = atom.clone();
00351   if (matcher_.empty() || atoms_.empty())
00352     return query;
00353   ImageAtom atom = _atom;
00354   pairs.clear();
00355   vector<DMatch> matches_all;
00356 
00357   vector<vector<DMatch> > knnmatches;
00358   int knn = 4;// + 2 * log(atoms_.size()) / log(2);
00359 
00360   vector<Mat> masks;
00361   //generateMasks(atom, masks);
00362   matcher_->knnMatch(atom.features().descriptors(),knnmatches,knn,masks);
00363   //matcher_->match(atom.features().descriptors(), matches_all, masks);
00364   flattenKNN(knnmatches, matches_all);
00365 
00366   for (size_t j = 0; j < atoms_.size(); j++)
00367   {
00368     vector<DMatch> matches;
00369     condenseByTrainImgIdx(matches_all, j, matches);
00370     if (matches.size() >= 2)
00371     {
00372       AtomPair atom_pair(atoms_[j], atom.ptrToSelf(), matches);
00373       pairs.push_back(atom_pair);
00374     }
00375   }
00376 
00377   cout << "found " << pairs.size() << " to test." << endl;
00378   FitPair pair_fitter(fitter, -1, new list<AtomPair> ());
00379   std::for_each(pairs.begin(), pairs.end(), pair_fitter);
00380   if (clone)
00381   {
00382     if (pair_fitter.good_pairs->size() > 0)
00383     {
00384       query = atom.clone();
00385       std::for_each(pair_fitter.good_pairs->begin(), pair_fitter.good_pairs->end(), ptrReplace(query));
00386     }
00387   }
00388   pairs = *pair_fitter.good_pairs;
00389   cout << "found " << pairs.size() << " are good." << endl;
00390   return query;
00391 }
00392 void MoleculeGlob::addAtomDescriptors(cv::Ptr<ImageAtom> atom)
00393 {
00394   if (atom_uids_idxs_.count(atom->uid()))
00395     return;
00396   if (!atom.empty())
00397   {
00398     if (!atom->features().descriptors().empty())
00399     {
00400       all_descriptions_.push_back(atom->features().descriptors());
00401       atoms_.push_back(atom);
00402       if (matcher_.empty())
00403       {
00404         matcher_ = atom->features().makeMatcher();
00405         matcher_->clear();
00406       }
00407       matcher_->add(vector<Mat> (1, all_descriptions_.back()));
00408       matcher_->train();
00409     }
00410     atom_uids_idxs_[atom->uid()] = atoms_.size() - 1;
00411   }
00412 }
00413 
00414 void MoleculeGlob::setMatcher(cv::Ptr<cv::DescriptorMatcher> matcher)
00415 {
00416   matcher_ = matcher;
00417 }
00418 
00419 void MoleculeGlob::generateMasks(const ImageAtom& atom, std::vector<cv::Mat>& masks) const
00420 {
00421 
00422   masks.resize(atoms_.size());
00423   for (size_t i = 0; i < atoms_.size(); i++)
00424   {
00425     atoms_[i]->descriptorMatchMask(atom, masks[i], Mat(), 50);
00426   }
00427 
00428 }
00429 Ptr<ImageAtom> MoleculeGlob::addAtomToGlob(cv::Ptr<ModelFitter> fitter, const ImageAtom& atom)
00430 {
00431 
00432   std::list<AtomPair> pairs;
00433   Ptr<ImageAtom> query = queryAtomToGlob(fitter, atom, pairs);
00434 
00435   if (pairs.empty())
00436   {
00437     query = atom.clone();
00438     query->setUid(uid_count_++);
00439     // either there are no _molecules yet or I didn't match any existing ones
00440     Ptr<ImageMolecule> mol_new = new ImageMolecule();
00441     mol_new->insertAtom(query);
00442     molecules.insert(mol_new);
00443     addAtomDescriptors(query);
00444   }
00445   else
00446     addPrefittedPairs(pairs);
00447 
00448   return query;
00449 }
00450 
00451 struct PairGlobber
00452 {
00453   MoleculeGlob* glob;
00454   PairGlobber(MoleculeGlob* glob) :
00455     glob(glob)
00456   {
00457   }
00458   void operator()(const AtomPair & pair)
00459   {
00460     if (!pair.result().success())
00461     {
00462       return;
00463     }
00464 
00465     if (pair.atom1()->uid() < 0)
00466     {
00467       Ptr<ImageAtom> atom = pair.atom1();
00468       atom->setUid(glob->uid_count_++);
00469 
00470     }
00471 
00472     if (pair.atom2()->uid() < 0)
00473     {
00474       Ptr<ImageAtom> atom = pair.atom2();
00475       atom->setUid(glob->uid_count_++);
00476 
00477     }
00478 
00479     glob->addAtomDescriptors(pair.atom2());
00480     glob->addAtomDescriptors(pair.atom1());
00481     std::set<Ptr<ImageMolecule> > matched_mols;
00482     // iterate over the molecules, query with each, merge if multiple matches
00483     std::set<Ptr<ImageMolecule> >::iterator mol = glob->getMolecules().begin();
00484     for (; mol != glob->getMolecules().end(); ++mol)
00485     {
00486       Ptr<ImageMolecule> molecule = *mol;
00487       if (molecule->hasAtom(pair.atom1()) || molecule->hasAtom(pair.atom2()))
00488       {
00489         molecule->insertPair(pair);
00490         matched_mols.insert(molecule);
00491       }
00492     }
00493     if (matched_mols.empty())
00494     {
00495       ImageMolecule* mol = new ImageMolecule();
00496       mol->insertPair(pair);
00497       glob->addMolecule(mol);
00498     }
00499     else
00500     {
00501 
00502       Ptr<ImageMolecule> match_mol_first = *matched_mols.begin();
00503 
00504       matched_mols.erase(match_mol_first);
00505       while (matched_mols.size())
00506       {
00507 
00508         Ptr<ImageMolecule> mol = *matched_mols.begin();
00509         Ptr<ImageAtom> shared = match_mol_first->hasAtom(pair.atom1()) ? pair.atom1() : pair.atom2();
00510         match_mol_first->merge(shared, *mol);
00511         matched_mols.erase(mol);
00512         glob->getMolecules().erase(mol);
00513       }
00514 
00515     }
00516   }
00517 
00518 };
00519 
00520 void MoleculeGlob::addPrefittedPairs(const std::list<AtomPair>& pairs, cv::Ptr<ImageAtom> atom)
00521 {
00522 
00523   for_each(pairs.begin(), pairs.end(), PairGlobber(this));
00524 }
00525 
00526 void MoleculeGlob::addPrefittedPairs(const std::vector<AtomPair>& pairs, cv::Ptr<ImageAtom> atom)
00527 {
00528   for_each(pairs.begin(), pairs.end(), PairGlobber(this));
00529 
00530 }
00531 
00532 cv::Ptr<ImageMolecule> MoleculeGlob::getBiggestMolecule() const
00533 {
00534 
00535   if (molecules.empty())
00536     return NULL;
00537   cv::Ptr<ImageMolecule> biggest;
00538 
00539   std::set<cv::Ptr<ImageMolecule> >::iterator it = molecules.begin();
00540   biggest = *(it);
00541   while (++it != molecules.end())
00542   { // try to keep molecule with the most atoms!
00543     if ((*it)->getAtoms().size() > biggest->getAtoms().size())
00544     {
00545       biggest = (*it);
00546     }
00547   }
00548   return biggest;
00549 }
00550 
00551 void MoleculeGlob::truncateMolecules(Ptr<ImageMolecule> mol_in)
00552 {
00553   if (mol_in.empty())
00554   {
00555     mol_in = getBiggestMolecule();
00556   }
00557   molecules.clear();
00558   molecules.insert(mol_in);
00559 }
00560 
00561 cv::Ptr<ImageMolecule> MoleculeGlob::getMerged() const
00562 {
00563   Ptr<ImageMolecule> globmol = new ImageMolecule();
00564   set<Ptr<ImageMolecule> > mols = getMolecules();
00565   while (!mols.empty())
00566   {
00567     Ptr<ImageMolecule> mol = *mols.begin();
00568 
00569     globmol->insertAtoms(mol->getAtoms());
00570     //todo verify that this is a bettter/worse option
00571     //globmol->insertPairs(mol->getPairs());
00572     mols.erase(mol);
00573   }
00574   return globmol;
00575 }
00576 
00577 namespace
00578 {
00579 struct BatchTrinsics
00580 {
00581   void operator()(Ptr<ImageMolecule> mol)
00582   {
00583     if (mol->getAtoms().size() < 2)
00584       return;
00585     mol->setAnchor(mol->getMaximallyConnectedAtom());
00586     MoleculeProcessor::findAndSetTrinsics(*mol, MoleculeProcessor::DIJKSTRA);
00587   }
00588 };
00589 struct BatchWrite
00590 {
00591   cv::FileStorage& fs_;
00592   BatchWrite(cv::FileStorage& fs) :
00593     fs_(fs)
00594   {
00595 
00596   }
00597   void operator()(Ptr<ImageMolecule> mol)
00598   {
00599     mol->serialize(fs_);
00600   }
00601 };
00602 
00603 struct DirOverrider
00604 {
00605   const std::string& dir;
00606   DirOverrider(const std::string& dir) :
00607     dir(dir)
00608   {
00609 
00610   }
00611 
00612   void operator()(Ptr<ImageAtom> atom)
00613   {
00614     atom->images().path() = dir;
00615   }
00616 
00617   void operator()(Ptr<ImageMolecule> mol)
00618   {
00619     for_each(mol->getAtoms().begin(), mol->getAtoms().end(), *this);
00620   }
00621 };
00622 
00623 }
00624 void MoleculeGlob::batchFindAndSetTrinsics()
00625 {
00626   for_each(molecules.begin(), molecules.end(), BatchTrinsics());
00627 }
00628 
00629 void MoleculeGlob::serialize(cv::FileStorage& fs) const
00630 {
00631   fs << "{";
00632   fs << "molecules";
00633   fs << "[";
00634   for_each(molecules.begin(), molecules.end(), BatchWrite(fs));
00635   fs << "]";
00636   fs << "}";
00637 }
00638 void MoleculeGlob::deserialize(const cv::FileNode& fn)
00639 {
00640 
00641   FileNode mols = fn["molecules"];
00642   CV_Assert(mols.type() == FileNode::SEQ)
00643     ;
00644 
00645   for (size_t i = 0; i < mols.size(); i++)
00646   {
00647     Ptr<ImageMolecule> mol(new ImageMolecule());
00648     mol->deserialize(mols[i]);
00649     addMolecule(mol);
00650   }
00651 
00652 }
00653 
00654 void MoleculeGlob::overideDirectory(std::string directory)
00655 {
00656   for_each(molecules.begin(), molecules.end(), DirOverrider(directory));
00657 }
00658 
00659 const std::string Globber::VERBOSE = "GlobberVerbose";
00660 void Globber::operator()(cv::Ptr<ImageAtom> atom)
00661 {
00662   glob->addAtomToGlob(fitter, *atom);
00663 }
00664 
00665 PairPointsCSV::PairPointsCSV(std::ostream&out) :
00666   out(out)
00667 {
00668 }
00669 
00670 void PairPointsCSV::operator()(const std::pair<const Point2f&, const Point2f&>& pp)
00671 {
00672 
00673   out << pp.first.x << "," << pp.first.y << "," << pp.second.x << "," << pp.second.y << std::endl;
00674 }
00675 void PairPointsCSV::operator()(const AtomPair&pair)
00676 {
00677   out << "##### " << pair.atom1()->images().fname() << "," << pair.atom2()->images().fname() << std::endl;
00678   const vector<Point2f>& pts1 = pair.pts1();
00679   const vector<Point2f>& pts2 = pair.pts1();
00680   for (size_t i = 0; i < pts1.size(); i++)
00681   {
00682 
00683     (*this)(std::make_pair(pts1[i], pts2[i]));
00684   }
00685 }
00686 
00687 int PairNodeData::max_depth = 0;
00688 int PairNodeData::min_depth = 0;
00689 
00690 void PairNode::setDist(const AtomPair& pair, const PairNode& prev_node)
00691 {
00692 
00693   float in_pair_error = error_func(pair, prev_node.node_data);
00694   float next_error_candidate = in_pair_error + prev_node.node_data.distance;
00695 
00696   if (this->node_data.distance > next_error_candidate)
00697   {
00698     this->min_prev = pair.other(this->atom);
00699     this->node_data.distance = next_error_candidate;
00700     Extrinsics& ext = atom->extrinsics();
00701 
00702     Mat R_increment = pair.TMtoOther(min_prev, FitterResult::R);
00703     Mat R_previous = min_prev->extrinsics().mat(Extrinsics::R).clone();
00704     ext.mat(Extrinsics::R) = R_increment * R_previous;
00705     Mat omega;
00706     cv::Rodrigues(ext.mat(Extrinsics::R), omega);
00707     ext.mat(Extrinsics::W) = omega;
00708 
00709     double err_new = in_pair_error;
00710     double err_prv = next_error_candidate - err_new;
00711     ext.flag(Extrinsics::ESTIMATED) = pair.result().success();
00712     ext.val(Extrinsics::CONFIDENCE) = bconf_func(err_new, err_prv);
00713 
00714     // update the node data
00715     this->node_data.depth = prev_node.node_data.depth + 1;
00716     this->node_data.mode = ext.flag(Extrinsics::ESTIMATED);
00717     if (this->node_data.depth > PairNodeData::max_depth)
00718     {
00719       PairNodeData::max_depth++;
00720     }
00721 
00722   }
00723 }
00724 
00725 std::ostream& operator <<(std::ostream& out, const PairNode& node)
00726 {
00727 
00728   std::string name1 = "TOP";
00729   std::string color_str = "color=black";
00730   if (node.min_prev)
00731   {
00732     name1 = (*node.min_prev).images().fname();
00733   }
00734   std::string name2 = "TOP";
00735   double dbl_conf = 0.0;
00736   if (node.atom)
00737   {
00738     name2 = (*node.atom).images().fname();
00739     dbl_conf = node.atom->extrinsics().val(Extrinsics::CONFIDENCE);
00740     bool tm_mode = node.atom->extrinsics().flag(Extrinsics::ESTIMATED);
00741     if (tm_mode)
00742     {
00743       color_str = "color=blue";
00744     }
00745     else// if (ImageAtomTrinsincs::NOT_TRUSTED == tm_mode)
00746     {
00747       color_str = "color=red";
00748     }
00749   }
00750   out << "node [" << color_str << ",fontname=Arial] \n" << "edge [color=black, style=solid] \n" //setup options
00751       << "\"" << name1 << "\" -> \"" << name2 << "\" [label=\"" << "E=" << std::setprecision(3)
00752       << node.node_data.distance << "  C=" << setprecision(3) << dbl_conf << "\", fontcolor=darkgreen];";
00753   return out;
00754 }
00755 
00756 std::ostream& operator <<(std::ostream& out, const AtomPair& pair)
00757 {
00758   std::string name1 = pair.atom1()->images().fname();
00759   std::string name2 = pair.atom2()->images().fname();
00760 
00761   // once trinsics are set, unless atom1 is up the chain from atom2,
00762   // the implicit R will not match the pair-wise R we found during matching!
00763   cv::Mat R2 = pair.atom2()->extrinsics().mat(Extrinsics::R);
00764   cv::Mat R1 = pair.atom1()->extrinsics().mat(Extrinsics::R);
00765   cv::Mat Rpair = pair.TMtoOther(pair.atom1(), Extrinsics::R);
00766   cv::Mat Rimplicit = R2 * R1.t();
00767 
00768   float reproj_err = calcReprojectonError(pair.pts1(), pair.pts2(), pair.result().inlier_mask(), Rimplicit,
00769                                           pair.atom1()->camera().K());
00770   float result_err = calcReprojectonError(pair.pts1(), pair.pts2(), pair.result().inlier_mask(), Rpair,
00771                                           pair.atom1()->camera().K());
00772 
00773   out << "node [color=black,fontname=Arial] \n" << "edge [color=blue, style=dashed] \n" //setup options
00774       << "\"" << name1 << "\" -> \"" << name2 << "\" [label=\"" << "PairError= " << result_err << " ImplicitError= "
00775       << reproj_err << "\", fontcolor=blue];";
00776   return out;
00777 }
00778 
00779 void PairNode::graphviz_dump(std::ostream& out, const std::map<cv::Ptr<ImageAtom>, PairNode>& node_map)
00780 {
00781   out << "digraph PairNodes{\n";
00782   std::map<cv::Ptr<ImageAtom>, PairNode>::const_iterator it = node_map.begin();
00783   while (it != node_map.end())
00784   {
00785     out << it->second << std::endl;
00786     ++it;
00787   }
00788   out << "}" << std::endl;
00789 }
00790 
00791 void PairNode::graphviz_dump_all(std::ostream& out, const std::map<cv::Ptr<ImageAtom>, PairNode>& node_map)
00792 {
00793   out << "digraph PairNodes{\n";
00794   std::map<cv::Ptr<ImageAtom>, PairNode>::const_iterator it = node_map.begin();
00795   while (it != node_map.end())
00796   {
00797 
00798     if (it->first)
00799     {
00800       AtomPairSet pairs = it->second.node_data.neighbors;
00801       AtomPairSet::iterator pit = pairs.begin();
00802 
00803       for (; pit != pairs.end(); ++pit)
00804       {
00805         out << *pit << std::endl;
00806       }
00807     }
00808     out << it->second << std::endl;
00809     ++it;
00810   }
00811   out << "}" << std::endl;
00812 }
00813 
00814 std::string strip(const std::string& str, const std::string & what)
00815 {
00816   return str.substr(0, str.find(what));
00817 }
00818 
00819 } // end namespace pano


pano_core
Author(s): Ethan Rublee
autogenerated on Mon Mar 14 2016 10:56:54