ModelFitter.h
Go to the documentation of this file.
00001 /*
00002  * ModelFitter.h
00003  *
00004  *  Created on: Jun 5, 2010
00005  *      Author: ethan
00006  */
00007 
00008 #ifndef PANO_MODELFITTER_H_
00009 #define PANO_MODELFITTER_H_
00010 
00011 #include <iostream>
00012 
00013 #include <string>
00014 #include <utility>
00015 
00016 #include <vector>
00017 #include <list>
00018 #include <map>
00019 #include <set>
00020 
00021 #include <stdexcept>
00022 #include <exception>
00023 
00024 #include <opencv2/core/core.hpp>
00025 #include <opencv2/features2d/features2d.hpp>
00026 #include <opencv2/calib3d/calib3d.hpp>
00027 
00028 #include <pano_core/pano_interfaces.h>
00029 #include <pano_core/ImageAtom.h>
00030 #include <pano_core/callbacks.h>
00031 
00032 namespace pano
00033 {
00034 
00038 class FitterResult : public serializable
00039 {
00040 public:
00041 
00042   FitterResult();
00043   FitterResult(const std::vector<cv::Mat>& mats, bool success, double err, double err_thresh,
00044                const std::vector<uchar>& inlier_mask, size_t inliers);
00045   virtual ~FitterResult()
00046   {
00047   }
00048 
00049   bool success() const
00050   {
00051     return success_;
00052   }
00053 
00054   bool empty() const
00055   {
00056     return empty_;
00057   }
00058 
00059   const cv::Mat& mat(size_t which) const
00060   {
00061     CV_Assert(which < mats_.size())
00062       ;
00063     return mats_[which];
00064   }
00065 
00066   double err() const
00067   {
00068     return err_;
00069   }
00070   double err_thresh() const
00071   {
00072     return err_thresh_;
00073   }
00074   const std::vector<uchar>& inlier_mask() const
00075   {
00076     return inlier_mask_;
00077   }
00078   size_t inliers() const
00079   {
00080     return inliers_;
00081   }
00082 
00083   virtual int version() const
00084   {
00085     return 0;
00086   }
00087   virtual void serialize(cv::FileStorage& fs) const;
00088   virtual void deserialize(const cv::FileNode& fn);
00089 
00090   const char* getMatName(int which) const;
00091   void setNonStdMatName(int idx, const std::string& name);
00092 
00093   enum StdMats
00094   {
00095     R = 0, 
00096     W_HAT = 1, 
00097     T = 2, 
00098     N_MATS
00100   };
00101 
00102   static const char* GetStdMatName(int which);
00103   static std::vector<cv::Mat> GenerateStdMats();
00104 
00105 private:
00106   std::vector<cv::Mat> mats_;
00107   bool success_;
00108   double err_;
00109   double err_thresh_;
00110   std::vector<uchar> inlier_mask_;
00111   size_t inliers_; 
00112   bool empty_;
00113 
00114   std::vector<std::string> names_;
00115 };
00116 
00117 struct AtomPair : public serializable
00118 {
00119   AtomPair();
00120 
00121   AtomPair(const cv::Ptr<ImageAtom>& atom1, const cv::Ptr<ImageAtom>& atom2, const std::vector<cv::Point2f>& points1,  const std::vector<cv::Point2f>& points2);
00122   AtomPair(const cv::Ptr<ImageAtom>& atom1, const cv::Ptr<ImageAtom>& atom2, const std::vector<cv::DMatch>& matches);
00123 
00124   void setAtom1(const cv::Ptr<ImageAtom>& atom)
00125   {
00126     atom1_ = atom;
00127   }
00128   void setAtom2(const cv::Ptr<ImageAtom>& atom)
00129   {
00130     atom2_ = atom;
00131   }
00132 
00133   const cv::Ptr<ImageAtom>& atom1() const
00134   {
00135     return atom1_;
00136   }
00137   const cv::Ptr<ImageAtom>& atom2() const
00138   {
00139     return atom2_;
00140   }
00141 
00142   const cv::Ptr<ImageAtom>& other(const cv::Ptr<ImageAtom>& atom) const
00143   {
00144     return atom == atom1_ ? atom2_ : atom1_;
00145   }
00146 
00147   Extrinsics generateExtrinsics(const cv::Ptr<ImageAtom>& atom) const
00148   {
00149     Extrinsics ext = atom->extrinsics();
00150     ext.flag(Extrinsics::ESTIMATED) = result_->success();
00151     if (ext.flag(Extrinsics::ESTIMATED))
00152     {
00153       ext.mat(Extrinsics::R) = RofThis(atom);
00154       cv::Rodrigues(ext.mat(Extrinsics::R), ext.mat(Extrinsics::W));
00155 
00156       ext.val(Extrinsics::CONFIDENCE) = result().err();
00157 
00158       ext.val(Extrinsics::CONFIDENCE) += other(atom)->extrinsics().val(Extrinsics::CONFIDENCE);
00159     }
00160 
00161     return ext;
00162   }
00163 
00164   cv::Mat RofThis(const cv::Ptr<ImageAtom>& atom) const
00165   {
00166     cv::Mat R_12 = TMtoOther(other(atom), FitterResult::R);
00167     cv::Mat R_1 = other(atom)->extrinsics().mat(Extrinsics::R);
00168     return R_12 * R_1;
00169   }
00170   cv::Mat TMtoOther(const cv::Ptr<ImageAtom>& atom, int which) const
00171   {
00172     return atom == atom2_ ? result_->mat(which).t() : result_->mat(which);
00173   }
00174 
00175   cv::Mat homographyFromQueryToOther(const cv::Ptr<ImageAtom>& query_p) const
00176   {
00177     const ImageAtom& query = *query_p, &prior = *other(query_p);
00178     cv::Mat R21 = TMtoOther(query_p, Extrinsics::R);
00179     cv::Mat H = query.camera().K() * R21 * prior.camera().Kinv();
00180     return H;
00181   }
00182 
00183   void setMatches(const std::vector<cv::DMatch>& matches);
00184   const std::vector<cv::DMatch>& getMatches() const;
00185   FitterResult & result();
00186   const FitterResult & result() const;
00187 
00188   const std::vector<cv::Point2f>& pts1() const
00189   {
00190     return *pts1_;
00191   }
00192   const std::vector<cv::Point2f>& pts2() const
00193   {
00194     return *pts2_;
00195   }
00196   const std::vector<cv::Point3f>& rays1() const
00197   {
00198     return *rays1_;
00199   }
00200   const std::vector<cv::Point3f>& rays2() const
00201   {
00202     return *rays2_;
00203   }
00204   const std::vector<cv::DMatch>& matches() const
00205   {
00206     return *matches_;
00207   }
00208 
00209   bool operator==(const AtomPair& rhs) const
00210   {
00211     return atom1_ == rhs.atom1_ && atom2_ == rhs.atom2_;
00212   }
00213 
00214   virtual int version() const
00215   {
00216     return 0;
00217   }
00218 
00219   void drawMatches(cv::Mat& out) const;
00220   virtual void serialize(cv::FileStorage& fs) const;
00221   virtual void deserialize(const cv::FileNode& fn);
00222 
00223 private:
00224 
00225   cv::Ptr<ImageAtom> atom1_, atom2_;
00226   cv::Ptr<std::vector<cv::DMatch> > matches_;
00227   cv::Ptr<FitterResult> result_;
00228   cv::Ptr<std::vector<cv::Point2f> > pts1_, pts2_;
00229   cv::Ptr<std::vector<cv::Point3f> > rays1_, rays2_;
00230 };
00231 
00232 struct AtomPairLess
00233 {
00234   bool operator()(const AtomPair& lhs, const AtomPair& rhs) const
00235   {
00236     // look at std::pair for this implementation
00237     //    __x.first < __y.first
00238     //                 || (!(__y.first < __x.first) && __x.second < __y.second)
00239     return lhs.atom1() < rhs.atom1() || (!(rhs.atom1() < lhs.atom1()) && lhs.atom2() < rhs.atom2());
00240   }
00241 };
00242 struct AtomPairMin
00243 {
00244   bool operator()(const AtomPair& lhs, const AtomPair& rhs)
00245   {
00246     return lhs.result().err() < rhs.result().err();
00247   }
00248 };
00249 struct AtomPairDist
00250 {
00251   float operator()(const AtomPair& p) const
00252   {
00253     return norm(p.result().mat(FitterResult::W_HAT));
00254   }
00255   bool operator()(const AtomPair& lhs, const AtomPair& rhs) const
00256   {
00257     return (*this)(lhs) < (*this)(rhs);
00258   }
00259 };
00260 
00261 typedef std::set<AtomPair, AtomPairLess> AtomPairSet;
00262 
00263 struct PairPointsCSV
00264 {
00265   std::ostream& out;
00266 
00267   PairPointsCSV(std::ostream&out);
00268 
00269   void operator()(const std::pair<const cv::Point2f&, const cv::Point2f&>& pp);
00270   void operator()(const AtomPair&pair);
00271 };
00272 
00273 class ModelFitterParams
00274 {
00275 public:
00276   virtual ~ModelFitterParams()
00277   {
00278   }
00279 
00280   template<typename T>
00281     const T& cast() const
00282     {
00283       dynamic_cast<const T&> (*this);
00284     }
00285   template<typename T>
00286     T& cast()
00287     {
00288       dynamic_cast<T&> (*this);
00289     }
00290 };
00291 
00292 class ModelFitter
00293 {
00294 public:
00295 
00296   virtual ~ModelFitter()
00297   {
00298   }
00299 
00300   void fit(AtomPair& pair);
00301 
00302 protected:
00303 
00304   virtual void fit_impl(AtomPair& pair) = 0;
00305 
00306 };
00307 
00308 template<typename Fitter, typename Params>
00309   class GenericModelFitter : public ModelFitter
00310   {
00311   public:
00312     GenericModelFitter() :
00313       fitter_(), params_()
00314     {
00315     }
00316 
00317     GenericModelFitter(const Params& params) :
00318       fitter_(), params_(params)
00319     {
00320     }
00321 
00322   protected:
00323     virtual void fit_impl(AtomPair& pair)
00324     {
00325       fitter_(params_, pair);
00326     }
00327   private:
00328     Fitter fitter_;
00329     Params params_;
00330   };
00331 
00332 struct FitPair
00333 {
00334   const static std::string VERBOSE;
00335   const static std::string UBER_VERBOSE;
00336 
00337   cv::Ptr<ModelFitter> fitter;
00338   cv::Ptr<int> failcount;
00339   cv::Ptr<int> totalcount;
00340 
00341   int fail_limit;
00342 
00343   cv::Ptr<std::list<AtomPair> > good_pairs;
00344   CallbackEngine* cbengine;
00345 
00346   FitPair(cv::Ptr<ModelFitter> fitter, int fail_limit, cv::Ptr<std::list<AtomPair> > good_pairs,CallbackEngine*cbengine=NULL);
00347 
00348   void operator()(AtomPair &pair);
00349 };
00350 
00353 void convertRTtoG(const cv::Mat& R, const cv::Mat& T, cv::Mat& G);
00354 
00359 float calcError(const cv::Point3f& p1, const cv::Point3f& p2, const cv::Mat& R, const cv::Mat& K);
00360 
00361 float calcError(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2,
00362                 const std::vector<uchar> & mask, const cv::Mat& R, const cv::Mat& K, int norm_type = cv::NORM_L1);
00363 
00367 float calcReprojectonError(const std::vector<cv::Point2f>& pts1, const std::vector<cv::Point2f>& pts2,
00368                            const std::vector<uchar> & mask, const cv::Mat& R, const cv::Mat& K, int norm_type =
00369                                cv::NORM_L1);
00370 
00373 void sparsifyMatches(const ImageAtom& atom1, const ImageAtom& atom2, std::vector<cv::Point2f>& pts1, std::vector<
00374     cv::Point2f>& pts2, int iKeep = 50);
00375 
00378 void sparsifyMatches(AtomPair& pair, int iKeep = 50);
00379 
00380 inline void point2fto3dMat(const cv::Point2f& p1, const cv::Mat& Kinv, cv::Mat& p13d)
00381 {
00382   p13d = Kinv * (cv::Mat_<float>(3, 1) << p1.x, p1.y, 1.0f);
00383   p13d /= norm(p13d);
00384 }
00385 
00386 inline cv::Mat point2fto3dMat(const cv::Point2f& p1, const cv::Mat& Kinv)
00387 {
00388   cv::Mat p13d;
00389   point2fto3dMat(p1, Kinv, p13d);
00390   return p13d;
00391 }
00394 inline cv::Mat pt2f_to_3d(const cv::Point2f& p1, const cv::Mat& K)
00395 {
00396   return point2fto3dMat(p1, K.inv());
00397 }
00398 
00399 inline cv::Point3f point2fTo3f(const cv::Point2f& p1, const cv::Mat& Kinverse)
00400 {
00401   assert(Kinverse.type() == CV_32F);
00402   cv::Mat p13d = Kinverse * (cv::Mat_<float>(3, 1) << p1.x, p1.y, 1.0f);
00403   p13d /= norm(p13d);
00404   return cv::Point3f(p13d.at<float> (0), p13d.at<float> (1), p13d.at<float> (2));
00405 }
00406 
00407 inline cv::Point2f point3fTo2f(const cv::Point3f& p1, const cv::Mat& K)
00408 {
00409   assert(K.type() == CV_32F);
00410   cv::Mat p12d = K * (cv::Mat_<float>(3, 1) << p1.x, p1.y, p1.z);
00411   p12d /= p12d.at<float> (2);
00412   return p12d.at<cv::Point2f> (0);
00413 }
00414 
00415 template<typename Inserter, typename Begin, typename End>
00416   void points2fto3f(Begin begin, End end, Inserter it, const cv::Mat& Kinverse)
00417   {
00418     while (begin != end)
00419     {
00420       *it = point2fTo3f(*begin, Kinverse);
00421       ++it;
00422       ++begin;
00423     }
00424   }
00425 
00426 template<typename Inserter, typename Begin, typename End>
00427   void points3fto2f(Begin begin, End end, Inserter it, const cv::Mat& K)
00428   {
00429     while (begin != end)
00430     {
00431       *it = point3fTo2f(*begin, K);
00432       ++it;
00433       ++begin;
00434     }
00435   }
00436 
00437 struct SVDRSolverParams : public serializable
00438 {
00439   //  params.error_thresh = 5;
00440   //  params.inliers_thresh = 15;
00441   //  params.maxiters = 30;
00442   //  params.nNeeded = 2;
00443   SVDRSolverParams() :
00444     maxiters(30), error_thresh(5), inliers_thresh(15), nNeeded(2)
00445   {
00446 
00447   }
00448 
00449   virtual int version() const
00450   {
00451     return 0;
00452   }
00453   virtual void serialize(cv::FileStorage& fs) const;
00454   virtual void deserialize(const cv::FileNode& fn);
00455 
00456   int maxiters;
00457   double error_thresh;
00458   double inliers_thresh;
00459   int nNeeded;
00460 };
00461 
00462 struct SVDFitR
00463 {
00464   void operator()(SVDRSolverParams& params, AtomPair& pair) const;
00465 };
00466 
00467 typedef GenericModelFitter<SVDFitR, SVDRSolverParams> SVDFitter;
00468 }
00469 #endif /* PANO_MODELFITTER_H_ */


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