ModelFitter.cpp
Go to the documentation of this file.
00001 /*
00002  * ModelFitter.cpp
00003  *
00004  *
00005  *
00006  */
00007 
00008 #include <stdint.h>
00009 #include <iostream>
00010 #include <algorithm>
00011 
00012 #include <pano_core/ModelFitter.h>
00013 #include <pano_core/feature_utils.h>
00014 
00015 #include <opencv2/highgui/highgui.hpp>
00016 
00017 using namespace cv;
00018 
00019 namespace pano
00020 {
00021 const char* FitterResult::GetStdMatName(int which)
00022 {
00023   switch (which)
00024   {
00025     case R:
00026       return "R";
00027     case W_HAT:
00028       return "W_HAT";
00029     case T:
00030       return "T";
00031     default:
00032       return "unknown";
00033   }
00034 }
00035 
00036 std::vector<cv::Mat> FitterResult::GenerateStdMats()
00037 {
00038   std::vector<cv::Mat> mats(N_MATS);
00039   mats[R] = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00040   mats[W_HAT] = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
00041   mats[T] = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
00042   return mats;
00043 }
00044 
00045 FitterResult::FitterResult() :
00046   mats_(N_MATS), success_(false), err_(1.0e8), err_thresh_(0), inliers_(0), empty_(true)
00047 {
00048 }
00049 FitterResult::FitterResult(const std::vector<cv::Mat>& mats, bool success, double err, double err_thresh,
00050                            const std::vector<uchar>& inlier_mask, size_t inliers) :
00051   mats_(mats), success_(success), err_(err), err_thresh_(err_thresh), inlier_mask_(inlier_mask), inliers_(inliers),
00052       empty_(false), names_(mats_.size())
00053 {
00054 
00055 }
00056 const char* FitterResult::getMatName(int which) const
00057 {
00058   if (!names_[which].empty())
00059     return names_[which].c_str();
00060   else
00061     return GetStdMatName(which);
00062 }
00063 void FitterResult::setNonStdMatName(int idx, const std::string& name)
00064 {
00065   names_[idx] = name;
00066 }
00067 void FitterResult::serialize(cv::FileStorage& fs) const
00068 {
00069   fs << "{";
00070   cvWriteComment(*fs, "FitterResult class", 0);
00071   fs << "mats" << "[";
00072   for (size_t i = 0; i < mats_.size(); ++i)
00073   {
00074     if(!mats_[i].empty()){
00075       cvWriteComment(*fs, getMatName(i), 0);
00076       fs << mats_[i];
00077     }
00078 //    else
00079 //      fs << "[]";
00080   }
00081   fs << "]";
00082   fs << "names" << "[";
00083   for (size_t i = 0; i < names_.size(); ++i)
00084   {
00085     fs << getMatName(i);
00086   }
00087   fs << "]";
00088   fs << "empty" << (int)empty_;
00089   fs << "success" << (int)success_;
00090   fs << "inliers" << (int)inliers_;
00091   fs << "err" << err_;
00092   fs << "err_thresh" << err_thresh_;
00093   //fs << "inlier_mask" << result.inlier_mask;
00094   fs << "}";
00095 }
00096 void FitterResult::deserialize(const cv::FileNode& node)
00097 {
00098   FileNode mats = node["mats"];
00099   CV_Assert(mats.type() == FileNode::SEQ)
00100     ;
00101   mats_.resize(mats.size());
00102   for (size_t i = 0; i < mats.size(); i++)
00103   {
00104     mats[i] >> mats_[i];
00105   }
00106 
00107   FileNode names = node["names"];
00108   CV_Assert(names.type() == FileNode::SEQ)
00109     ;
00110   names_.resize(names.size());
00111   for (size_t i = 0; i < names.size(); i++)
00112   {
00113     names_[i] = (string)names[i];
00114   }
00115   empty_ = (int)node["empty"];
00116   success_ = (int)node["success"];
00117   inliers_ = (int)node["inliers"];
00118   err_ = (double)node["err"];
00119   err_thresh_ = (double)node["err_thresh"];
00120 }
00121 
00122 AtomPair::AtomPair() :
00123   /* pair<cv::Ptr<ImageAtom>, cv::Ptr<ImageAtom> > (),*/atom1_(), atom2_(), matches_(new std::vector<DMatch>()), result_(new FitterResult),
00124       pts1_(new std::vector<cv::Point2f>()), pts2_(new std::vector<cv::Point2f>()),
00125       rays1_(new std::vector<cv::Point3f>()), rays2_(new std::vector<cv::Point3f>())
00126 {
00127 
00128 }
00129 
00130 AtomPair::AtomPair(const cv::Ptr<ImageAtom>& atom1, const cv::Ptr<ImageAtom>& atom2,
00131                    const std::vector<cv::Point2f>& points1, const std::vector<cv::Point2f>& points2) :
00132   atom1_(atom1), atom2_(atom2), result_(new FitterResult()), pts1_(new std::vector<cv::Point2f>(points1)),
00133       pts2_(new std::vector<cv::Point2f>(points2)), rays1_(new std::vector<cv::Point3f>(points1.size())),
00134       rays2_(new std::vector<cv::Point3f>(points2.size()))
00135 {
00136 
00137   points2fto3f(pts1_->begin(), pts1_->end(), rays1_->begin(), atom1_->camera().Kinv());
00138   points2fto3f(pts2_->begin(), pts2_->end(), rays2_->begin(), atom2_->camera().Kinv());
00139 }
00140 AtomPair::AtomPair(const Ptr<ImageAtom>& atom1, const Ptr<ImageAtom>& atom2, const std::vector<cv::DMatch>& matches) :
00141   atom1_(atom1), atom2_(atom2), matches_(new vector<DMatch> (matches)), result_(new FitterResult()),
00142       pts1_(new std::vector<cv::Point2f>()), pts2_(new std::vector<cv::Point2f>()),
00143       rays1_(new std::vector<cv::Point3f>()), rays2_(new std::vector<cv::Point3f>())
00144 {
00145   matches2points(atom1_->features().kpts(), atom2_->features().kpts(), *matches_, *pts1_, *pts2_);
00146 
00147   rays1_->resize(pts1_->size());
00148   rays2_->resize(pts2_->size());
00149   points2fto3f(pts1_->begin(), pts1_->end(), rays1_->begin(), atom1_->camera().Kinv());
00150   points2fto3f(pts2_->begin(), pts2_->end(), rays2_->begin(), atom2_->camera().Kinv());
00151 }
00152 
00153 void AtomPair::setMatches(const std::vector<cv::DMatch>& matches){
00154   *matches_ = matches;
00155   matches2points(atom1_->features().kpts(), atom2_->features().kpts(), matches, *pts1_, *pts2_);
00156   rays1_->resize(pts1_->size());
00157   rays2_->resize(pts2_->size());
00158   points2fto3f(pts1_->begin(), pts1_->end(), rays1_->begin(), atom1_->camera().Kinv());
00159   points2fto3f(pts2_->begin(), pts2_->end(), rays2_->begin(), atom2_->camera().Kinv());
00160 }
00161 const std::vector<cv::DMatch>& AtomPair::getMatches() const
00162 {
00163   return *matches_;
00164 }
00165 FitterResult & AtomPair::result()
00166 {
00167   return *result_;
00168 }
00169 const FitterResult & AtomPair::result() const
00170 {
00171   return *result_;
00172 }
00173 
00174 void AtomPair::serialize(cv::FileStorage& fs) const
00175 {
00176   fs << "{";
00177   fs << "atom1";
00178   fs << atom1_->uid();
00179   fs << "atom2";
00180   fs << atom2_->uid();
00181   fs << "result";
00182   result_->serialize(fs);
00183   fs << "}";
00184 }
00185 void AtomPair::deserialize(const cv::FileNode& fn)
00186 {
00187   if (atom1_.empty())
00188     atom1_ = new ImageAtom();
00189   atom1_->setUid(int(fn["atom1"]));
00190   if (atom2_.empty())
00191     atom2_ = new ImageAtom();
00192   atom2_->setUid(int(fn["atom2"]));
00193   result_->deserialize(fn["result"]);
00194 }
00195 
00196 void AtomPair::drawMatches(cv::Mat& out) const{
00197 
00198   cv::drawMatches(atom2_->images().src(),atom2_->features().kpts(), atom1_->images().src(),atom1_->features().kpts(),*matches_,out);
00199 
00200 }
00201 namespace
00202 {
00203 
00204 inline Mat reprojectPoint(const Mat& p13d, const Mat& p23d, const Mat& R, const Mat& K)
00205 {
00206 
00207   Mat p23dhat = R * p13d;
00208   Mat p2 = K * p23d;
00209   Mat p2hat = K * p23dhat;
00210   p2 /= p2.at<float> (2);
00211   p2hat /= p2hat.at<float> (2);
00212   return (p2 - p2hat); // return difference!
00213 }
00214 
00215 inline float calcErrorL2(const Mat& p13d, const Mat& p23d, const Mat& R, const Mat& K)
00216 {
00217   return norm(reprojectPoint(p13d, p23d, R, K), cv::NORM_L2);
00218 }
00219 
00220 float calcErrorL1(const Mat& p13d, const Mat& p23d, const Mat& R, const Mat& K)
00221 {
00222   return norm(reprojectPoint(p13d, p23d, R, K), cv::NORM_L1);
00223 }
00224 
00225 } // end anonymous namespace
00226 
00227 
00228 float calcError(const Point3f& p1, const Point3f& p2, const Mat& R, const Mat& K)
00229 {
00230   return calcErrorL1(Mat(p1), Mat(p2), R, K);
00231 }
00232 
00233 namespace
00234 {
00235 
00236 typedef float (*calcErr_ptr)(const Mat& /*p13d*/, const Mat& /*p23d*/, const Mat& /*R*/, const Mat& /*K*/);
00237 
00238 }
00239 
00240 float calcError(const std::vector<Point3f>& pts1, const std::vector<Point3f>& pts2, const std::vector<uchar> & mask,
00241                 const cv::Mat& R, const cv::Mat& K, int norm_type)
00242 {
00243   calcErr_ptr erf_ptr;
00244   erf_ptr = (norm_type == cv::NORM_L1) ? calcErrorL1 : calcErrorL2;
00245 
00246   float Esum = 0;
00247   int inliers = 0;
00248   for (size_t i = 0; i < pts1.size(); i++)
00249   {
00250     if (mask.empty() || mask[i])
00251     {
00252       Esum += erf_ptr(Mat(pts1[i]), Mat(pts2[i]), R, K);
00253       inliers++;
00254     }
00255   }
00256 
00257   Esum = (norm_type == cv::NORM_L1) ? Esum : sqrt(Esum);
00258   return Esum / inliers;
00259 
00260 }
00261 
00262 float calcReprojectonError(const vector<Point2f>& pts1, const vector<Point2f>& pts2, const std::vector<uchar> & mask,
00263                            const cv::Mat& R, const cv::Mat& _K, int norm_type)
00264 {
00265   calcErr_ptr erf_ptr;
00266   erf_ptr = (norm_type == cv::NORM_L1) ? calcErrorL1 : calcErrorL2;
00267   size_t num_pts = pts1.size();
00268   Mat Rinv = R.t();
00269   cv::Mat K;
00270   _K.convertTo(K, CV_32F);
00271   Mat Kinv = K.inv();
00272   int inliers = 0;
00273   float Esum = 0.0;
00274   CV_Assert(num_pts == pts2.size());
00275   //std::cout << "Kinv" << Kinv << std::endl;
00276   for (size_t k = 0; k < num_pts; k++)
00277   {
00278     if (mask.empty() || mask[k])
00279     {
00280       Mat p13d = point2fto3dMat(pts1[k], Kinv);
00281       Mat p23d = point2fto3dMat(pts2[k], Kinv); // raise 2d points uv2 to xyz2
00282 //      std::cout << "p1" << pts1[k] << "p2" << pts2[k] << std::endl;
00283 //      std::cout << "p13d" << p13d << " p23d" << p23d << std::endl;
00284       Esum += erf_ptr(p13d, p23d, R, K);
00285       inliers++;
00286     }
00287   }
00288   Esum = (norm_type == cv::NORM_L1) ? Esum : sqrt(Esum);
00289   if (inliers < 1.0)
00290   {
00291     inliers++;
00292   }
00293   return Esum / inliers;
00294 }
00295 
00296 void convertRTtoG(const cv::Mat& R, const cv::Mat& T, cv::Mat& G)
00297 {
00298   assert(R.type() == T.type() && R.type() == CV_32F && R.rows == T.rows && T.rows == 3 && T.cols == 1 && R.cols == 3);
00299   G = cv::Mat::eye(4, 4, CV_32F);
00300   for (int i = 0; i < 3; i++)
00301   {
00302     for (int j = 0; j < 3; j++)
00303     {
00304       G.at<float> (i, j) = R.at<float> (i, j);
00305     }
00306     G.at<float> (i, 3) = T.at<float> (i);
00307   }
00308 }
00309 
00310 void sparsifyMatches(AtomPair& pair, int iKeep)
00311 {
00312 
00313   //  Ptr<ImageAtom> atom1;
00314   //  Ptr<ImageAtom> atom2;
00315   //  atom1 = pair.atom1();
00316   //  atom2 = pair.atom2();
00317 
00318   //  bool atom1_has_trinsics = atom1->getTrinsics().isSet();
00319   //  bool atom2_has_trinsics = atom2->getTrinsics().isSet();
00320   //  if (!atom1_has_trinsics || !atom2_has_trinsics)
00321   //  {
00322   //    Lout(Logger::WARNING) << "one or both atoms doesn't have trinsics, not sparsifying..." << std::endl;
00323   //    return;
00324   //  }
00325   //  Mat R1 = Mat::eye(3, 3, atom1->getTrinsics().R.type());
00326   //  Mat R2 = atom2->getTrinsics().R * atom1->getTrinsics().R.t();
00327   //
00328   //  std::vector<Point3f> &rays1 = *pair.getRays1(), &rays2 = *pair.getRays2();
00329   //
00330   //  std::vector<std::pair<float, int/*idx*/> > reproj_errs(rays1.size());
00331   //  for (size_t i = 0; i < rays1.size(); ++i)
00332   //  {
00333   //    double dist = calcErrorL1(Mat(rays1[i]), Mat(rays2[i]), R2, atom1->getK());
00334   //    (reproj_errs[i]).first = dist;
00335   //    (reproj_errs[i]).second = i;
00336   //  }
00337   //  std::sort(reproj_errs.begin(), reproj_errs.end(), compareRealToIndexPair);
00338   //
00339   //  int ikeep = (int)reproj_errs.size() < iKeep ? reproj_errs.size() : iKeep;
00340   //  Ptr<vector<DMatch> > matches = new vector<DMatch> (ikeep);
00341   //  for (int i = 0; i < ikeep; ++i)
00342   //  {
00343   //    DMatch match = pair.getMatches()[reproj_errs[i].second];
00344   //    match.distance = reproj_errs[i].first;
00345   //    (*matches)[i] = match;
00346   //  }
00347   //  pair.setMatches(matches);
00348 
00349 }
00350 
00351 void ModelFitter::fit(AtomPair& pair)
00352 {
00353   fit_impl(pair);
00354 }
00355 
00356 const string FitPair::VERBOSE = "FitPair::VERBOSE";
00357 const string FitPair::UBER_VERBOSE = "FitPair::UBER_VERBOSE";
00358 
00359 FitPair::FitPair(cv::Ptr<ModelFitter> fitter, int fail_limit, cv::Ptr<std::list<AtomPair> > good_pairs,
00360                  CallbackEngine*cbengine) :
00361   fitter(fitter), failcount(new int(0)), totalcount(new int(0)), fail_limit(fail_limit), good_pairs(good_pairs),
00362       cbengine(cbengine)
00363 {
00364 }
00365 void FitPair::operator()(AtomPair &pair)
00366 {
00367 
00368   if (fail_limit > 0 && *failcount > fail_limit)
00369     return;
00370   (*totalcount)++;
00371   fitter->fit(pair);
00372 
00373   if (pair.result().success())
00374   {
00375     //    vector<Point2f> points(20);
00376     //    vector<Point2f> pointsH(20);
00377     //    Mat m_points(points);
00378     //    Size sz = pair.atom2()->camera().img_size();
00379     //    randu(m_points, Scalar(0, 0), Scalar(sz.width, sz.height));
00380     //
00381     //    Mat H = pair.homographyFromQueryToOther(pair.atom2());
00382     //    Mat tpoints(pointsH);
00383     //    perspectiveTransform(m_points, tpoints, H);
00384     //    Images r = pair.atom2()->images();
00385     //    Images l = pair.atom1()->images();
00386     //    r.restore();
00387     //    l.restore();
00388     //    Mat result;
00389     //
00390     //    for (int i = 0; i < points.size(); i++)
00391     //    {
00392     //      Rect imroi(0, 0, sz.width, sz.height);
00393     //      Rect roir(points[i], Size(9, 9));
00394     //      Rect roil(pointsH[i]/*.x - 3, pointsH[i].y - 3*/, Size(9, 9));
00395     //
00396     //      if (imroi.contains(roir.tl()) && imroi.contains(roir.br()) && imroi.contains(roil.tl())
00397     //          && imroi.contains(roil.br()))
00398     //      {
00399     //        matchTemplate(l.grey()(roil), r.grey()(roir), result, CV_TM_SQDIFF);
00400     //        cout << "diff: " << result.at<float>(0) << endl;
00401     //      }
00402     //
00403     //    }
00404 
00405     //    int i_inliers = pair.result().inliers();
00406     //    double i_err = pair.result().err();
00407     //    cout << "error initial " << i_err <<endl;
00408     //    Mat H = pair.homographyFromQueryToOther(pair.atom2());
00409     //    vector<DMatch> matches;
00411     //   // sort(matches.begin(),matches.end());
00412     //   // matches.resize(matches.size()/2);
00413     //    pair = AtomPair(pair.atom1(),pair.atom2(),matches);
00414     //    fitter->fit(pair);
00415     //
00416     //    double p_err = pair.result().err();
00417     //    cout << "error post " << p_err <<endl;
00418     //    if(pair.result().success())
00419     good_pairs->push_back(pair);
00420     // IFLOG(VERBOSE, waitKey(10));
00421   }
00422   if (cbengine)
00423     cbengine->callBack(pair, 0);
00424 
00425 }
00426 }
00427 


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