SVDRSolver.cpp
Go to the documentation of this file.
00001 /*
00002  * SVDRSolver.cpp
00003  *
00004  *  Created on: Oct 19, 2010
00005  *      Author: erublee
00006  */
00007 #include "pano_core/ModelFitter.h"
00008 #include "pano_core/feature_utils.h"
00009 using namespace cv;
00010 
00011 #define ROOT_2 1.41421356
00012 #define EPS_EQ(t,val) (t >= val - 1.0e-3 && t <= val + 1.0e-3)
00013 
00014 namespace pano
00015 {
00016 
00017 // SVDRSolverParams params
00018 //int maxiters;
00019 // double error_thresh;
00020 // double inliers_thresh;
00021 // int nNeeded;
00022 
00023 void SVDRSolverParams::serialize(cv::FileStorage& fs) const
00024 {
00025   fs << "{";
00026   fs << "error_thresh" << error_thresh;
00027   fs << "inliers_thresh" << inliers_thresh;
00028   fs << "maxiters" << maxiters;
00029   fs << "nNeeded" << nNeeded;
00030   fs << "}";
00031 }
00032 void SVDRSolverParams::deserialize(const cv::FileNode& node)
00033 {
00034   error_thresh = (double)node["error_thresh"];
00035   inliers_thresh = (double)node["inliers_thresh"];
00036   maxiters = (int)node["maxiters"];
00037   nNeeded = (int)node["nNeeded"];
00038 }
00039 
00040 namespace
00041 {
00042 
00043 Mat svdsolve(const cv::Mat& correlation)
00044 {
00045   //solve for r using svd
00046   cv::SVD svd(correlation);
00047 
00048   //sgn of the determinant
00049   int s = cv::determinant(svd.u * svd.vt) > 0 ? 1 : -1;
00050 
00051   cv::Mat diagm = cv::Mat::eye(3, 3, svd.u.type());
00052 
00053   //create a matrix with all ones but the lower right corner = S
00054   diagm.at<float> (2, 2) = s;
00055 
00056   //according to the paper TM = U * diag(1,1,s) * V^T
00057   return svd.u * diagm * svd.vt;
00058 }
00059 
00060 Mat svdcorrelation(const vector<Point3f>& pts1, const vector<Point3f>& pts2, const vector<uchar>& mask)
00061 {
00062   cv::Mat r1 = cv::Mat(3, 1, CV_32F);
00063   cv::Mat r2 = cv::Mat(3, 1, CV_32F);
00064   // compute cross correlation estimate of bunch of 3-vectors
00065   cv::Mat correlation = cv::Mat::zeros(3, 3, cv::DataType<float>::type);
00066   float* r1p = r1.ptr<float> (0);
00067   float* r2p = r2.ptr<float> (0);
00068   for (size_t i = 0; i < pts1.size(); i++)
00069   {
00070     if (mask.empty() || mask[i])
00071     {
00072       memcpy(r1p, &pts1[i], 3 * sizeof(float));
00073       memcpy(r2p, &pts2[i], 3 * sizeof(float));
00074       //add to the correlation matrix C = sum( x x' )
00075       correlation += r2 * r1.t();
00076 
00077     }
00078   }
00079   return correlation;
00080 }
00081 
00082 Mat svdsolve(const vector<Point3f>& pts1, const vector<Point3f>& pts2, const vector<uchar>& mask)
00083 {
00084   return svdsolve(svdcorrelation(pts1, pts2, mask));
00085 }
00086 
00087 Mat svdsolve(const vector<Point2f>& pts1, const vector<Point2f>& pts2, const vector<uchar>& mask, const Mat& K)
00088 {
00089   // form the estimate covariance matrix from data points, sum( x x' )
00090   cv::Mat correlation = cv::Mat::zeros(3, 3, cv::DataType<float>::type);
00091   for (size_t i = 0; i < pts1.size(); i++)
00092   {
00093     if (mask.empty() || mask[i])
00094     {
00095       //create a 1X3 matrix for the img1 point
00096       cv::Mat r1 = pt2f_to_3d(pts1[i], K);
00097 
00098       //create a 3x1 matrix for the img2 point
00099       cv::Mat r2 = pt2f_to_3d(pts2[i], K);
00100 
00101       //add to the correlation matrix
00102       correlation += r2 * r1.t();
00103     }
00104   }
00105 
00106   return svdsolve(correlation);
00107 
00108 }
00109 
00110 struct NextNum
00111 {
00112   int num_;
00113   NextNum() :
00114     num_(0)
00115   {
00116   }
00117   int operator()()
00118   {
00119     return num_++;
00120   }
00121 };
00122 
00123 void ransacSolveR(const vector<Point3f>& pts1, const vector<Point3f>& pts2, FitterResult& result, int maxiters,
00124                   int inliers_thresh, int nNeeded, double error_thresh, cv::Mat K, cv::Mat Kinv)
00125 {
00126   float bestdist = 1e8;
00127   int bestinliers = 0;
00128   if (pts1.empty() || pts1.size() < size_t(inliers_thresh))
00129   {
00130     result = FitterResult(FitterResult::GenerateStdMats(), false, bestdist, error_thresh, vector<uchar> (), 0);
00131     return;
00132   }
00133 
00134   int iters = 0;
00135 
00136   vector<uchar> _best(pts1.size()), _mask(pts1.size());
00137 
00138   vector<uchar> * best = &_best;
00139   vector<uchar> * mask = &_mask;
00140 
00141   Mat bestR = Mat::eye(3, 3, DataType<float>::type);
00142 
00143   vector<Point3f> rpts1(nNeeded);
00144   vector<Point3f> rpts2(nNeeded);
00145 
00146   int ntotal = pts1.size();
00147   ;
00148   Mat R = cv::Mat::eye(3, 3, CV_32F);
00149 
00150   vector<int> idxs(pts1.size());
00151   generate(idxs.begin(), idxs.end(), NextNum());
00152 //  {
00153 //    Mat idxs_m(idxs);
00154 //    randShuffle(idxs_m);
00155 //  }
00156   int r_idx = 0;
00157 
00158   maxiters = std::min(int(pts1.size()),maxiters);
00159 
00160   while (iters++ < maxiters)
00161   {
00162 
00163     // there's no sense of whether or not we have 'converged' ?
00164     int numinliers = 0;
00165     float sumdist = 0;
00166 
00167     rpts1.clear();
00168     rpts2.clear();
00169 
00170     //select random subset
00171     while ((int)rpts1.size() < nNeeded)
00172     {
00173 
00174       if (find(rpts2.begin(), rpts2.end(), RadiusPoint(0.01, pts2[idxs[r_idx]])) == rpts2.end()
00175           && find(rpts1.begin(), rpts1.end(), RadiusPoint(0.01, pts1[idxs[r_idx]])) == rpts1.end())
00176       {
00177         rpts1.push_back(pts1[idxs[r_idx]]);
00178         rpts2.push_back(pts2[idxs[r_idx]]);
00179 
00180       }
00181       //cout << "idxs " << idxs[r_idx] << endl;
00182       r_idx++;
00183       if (r_idx >= (int)idxs.size())
00184       {
00185         break;
00186       }
00187 
00188     }
00189 
00190     if (r_idx >= (int)idxs.size())
00191     {
00192 
00193       r_idx = 0;
00194       Mat idxs_m(idxs);
00195       randShuffle(idxs_m);
00196       continue;
00197     }
00198 
00199     // fit TM to current inlier set via solver
00200     R = svdsolve(rpts1, rpts2, vector<uchar> ());
00201 
00202     std::fill(mask->begin(), mask->end(), 0);
00203     // for every point in the set, compute error per point
00204     for (int i = 0; i < ntotal; i++)
00205     {
00206       float error_point_i = calcError(pts1[i], pts2[i], R, K);
00207       if (error_point_i < error_thresh)
00208       { // if this point's error is good, keep as inlier
00209         numinliers++;
00210         (*mask)[i] = 1;
00211         sumdist += error_point_i;
00212       }
00213     }
00214     if (numinliers >= inliers_thresh)
00215     {
00216 
00217       Mat tR = svdsolve(pts1, pts2, *mask);
00218       float cur_dist = calcError(pts1, pts2, *mask, tR, K);
00219       //float cur_dist = sumdist/numinliers;
00220       if (cur_dist <= bestdist)
00221       {
00222         bestinliers = numinliers;
00223         vector<uchar> * temp = best;
00224         best = mask;
00225         mask = temp;
00226         bestR = tR;
00227         bestdist = cur_dist;
00228       }
00229 
00230     }
00231 
00232     if (inliers_thresh < bestinliers * 2){
00233      std::cout << "early break iters: " << iters << std::endl;
00234       break;
00235     }
00236 
00237   }
00238 
00239   cv::Mat omega_(3, 1, CV_32F);
00240   cv::Rodrigues(bestR, omega_);
00241   int good = bestinliers >= inliers_thresh;
00242   good = good * (bestdist < error_thresh);
00243   vector<Mat> mats = FitterResult::GenerateStdMats();
00244 
00245   mats[FitterResult::R] = bestR;
00246   mats[FitterResult::W_HAT] = omega_;
00247 
00248   result = FitterResult(mats, good, bestdist, error_thresh, *best, bestinliers);
00249 
00250 }
00251 
00252 void svdSolveR(const vector<Point3f>& rays1, const vector<Point3f>& rays2, FitterResult& result, int maxiters,
00253                int inliers_thresh, int nNeeded, double error_thresh, cv::Mat K, cv::Mat Kinv)
00254 {
00255 
00256   // fit TM to current inlier set via solver
00257   Mat R = svdsolve(rays1, rays2, vector<uchar> ());
00258   cv::Mat omega_(3, 1, CV_32F);
00259   cv::Rodrigues(R, omega_);
00260   vector<Mat> mats = FitterResult::GenerateStdMats();
00261 
00262   mats[FitterResult::R] = R;
00263   mats[FitterResult::W_HAT] = omega_;
00264 
00265   result = FitterResult(mats, true, 1.0, error_thresh, vector<uchar> (1,rays1.size()), rays1.size());
00266 
00267 }
00268 
00269 void ransacSolveR(const vector<Point2f>& _pts1, const vector<Point2f>& _pts2, FitterResult& result, int maxiters,
00270                int inliers_thresh, int nNeeded, double error_thresh, cv::Mat K, cv::Mat Kinv)
00271 {
00272 
00273   vector<Point3f> m_pts1, m_pts2;
00274   m_pts1.resize(_pts1.size());
00275   m_pts2.resize(_pts2.size());
00276   points2fto3f(_pts1.begin(), _pts1.end(), m_pts1.begin(), Kinv);
00277   points2fto3f(_pts2.begin(), _pts2.end(), m_pts2.begin(), Kinv);
00278   ransacSolveR(m_pts1, m_pts2, result, maxiters, inliers_thresh, nNeeded, error_thresh, K, Kinv);
00279 
00280 }
00281 
00282 }
00283 
00284 void SVDFitR::operator ()(SVDRSolverParams& params, AtomPair& pair) const
00285 {
00286 
00287   if (params.nNeeded < 0 && pair.rays1().size() >= 2)
00288   {
00289     svdSolveR(pair.rays1(), pair.rays2(), pair.result(), params.maxiters, params.inliers_thresh, params.nNeeded,
00290                   params.error_thresh, pair.atom1()->camera().K(), pair.atom1()->camera().Kinv());
00291     return;
00292   }
00293   if (pair.rays1().size() < params.inliers_thresh)
00294     return;
00295   ransacSolveR(pair.rays1(), pair.rays2(), pair.result(), params.maxiters, params.inliers_thresh, params.nNeeded,
00296                params.error_thresh, pair.atom1()->camera().K(), pair.atom1()->camera().Kinv());
00297 }
00298 
00299 }


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