35 #include <opencv2/core/core.hpp> 36 #include <opencv2/core/core_c.h> 37 #include <opencv2/calib3d/calib3d.hpp> 47 _matchCountMinAccepted(
Parameters::defaultVhEpMatchCountMin()),
48 _ransacParam1(
Parameters::defaultVhEpRansacParam1()),
49 _ransacParam2(
Parameters::defaultVhEpRansacParam2())
67 if(ssA == 0 || ssB == 0)
73 std::list<std::pair<int, std::pair<int, int> > > pairsId;
82 std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
83 for(std::list<std::pair<
int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
85 pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->
getWordsKpts()[iter->second.first], ssB->
getWordsKpts()[iter->second.second])));
88 std::vector<uchar> status;
91 int inliers =
uSum(status);
99 UDEBUG(
"inliers = %d/%d", inliers, pairs.size());
108 if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
114 if(fundamentalMatrix.type() != CV_64FC1)
121 cv::SVD svd(fundamentalMatrix);
129 e1[0] = v.at<
double>(0,2);
130 e1[1] = v.at<
double>(1,2);
131 e1[2] = v.at<
double>(2,2);
133 e2[0] = u.at<
double>(0,2);
134 e2[1] = u.at<
double>(1,2);
135 e2[2] = u.at<
double>(2,2);
141 cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
142 p0.at<
double>(0,0) = 1;
143 p0.at<
double>(1,1) = 1;
144 p0.at<
double>(2,2) = 1;
145 cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
146 p.at<
double>(0,0) = R.at<
double>(0,0);
147 p.at<
double>(0,1) = R.at<
double>(0,1);
148 p.at<
double>(0,2) = R.at<
double>(0,2);
149 p.at<
double>(1,0) = R.at<
double>(1,0);
150 p.at<
double>(1,1) = R.at<
double>(1,1);
151 p.at<
double>(1,2) = R.at<
double>(1,2);
152 p.at<
double>(2,0) = R.at<
double>(2,0);
153 p.at<
double>(2,1) = R.at<
double>(2,1);
154 p.at<
double>(2,2) = R.at<
double>(2,2);
155 p.at<
double>(0,3) = T.at<
double>(0,0);
156 p.at<
double>(1,3) = T.at<
double>(1,0);
157 p.at<
double>(2,3) = T.at<
double>(2,0);
163 cv::triangulatePoints(p0, p, x, xp, pts4D);
167 for(
int i=0; i<x.cols; ++i)
170 if(pts4D.at<
double>(2,i)/pts4D.at<
double>(3,i) > 5)
176 UDEBUG(
"nValid=%d/%d", nValid, x.cols);
190 UASSERT(E.rows == 3 && E.cols == 3);
192 UASSERT(x.rows == 2 && x.cols>0 && x.type() == CV_64FC1);
193 UASSERT(xp.rows == 2 && xp.cols>0 && x.type() == CV_64FC1);
196 cv::Mat w = cv::Mat::zeros( 3, 3, CV_64FC1);
197 w.at<
double>(0,1) = -1;
198 w.at<
double>(1,0) = 1;
199 w.at<
double>(2,2) = 1;
203 cv::SVD svd(e,cv::SVD::MODIFY_A);
213 cv::Mat diag = cv::Mat::eye(3,3,CV_64FC1);
214 diag.at<
double>(2,2) = 0;
216 svd(e,cv::SVD::MODIFY_A);
226 svd(e,cv::SVD::MODIFY_A);
237 cv::Mat r2 = u*wt*vt;
239 cv::Mat t1 = u.col(2);
240 cv::Mat t2 = u.col(2)*-1;
276 UDEBUG(
"Case %d", maxIndex);
279 cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
280 p.at<
double>(0,0) = R.at<
double>(0,0);
281 p.at<
double>(0,1) = R.at<
double>(0,1);
282 p.at<
double>(0,2) = R.at<
double>(0,2);
283 p.at<
double>(1,0) = R.at<
double>(1,0);
284 p.at<
double>(1,1) = R.at<
double>(1,1);
285 p.at<
double>(1,2) = R.at<
double>(1,2);
286 p.at<
double>(2,0) = R.at<
double>(2,0);
287 p.at<
double>(2,1) = R.at<
double>(2,1);
288 p.at<
double>(2,2) = R.at<
double>(2,2);
289 p.at<
double>(0,3) = T.at<
double>(0);
290 p.at<
double>(1,3) = T.at<
double>(1);
291 p.at<
double>(2,3) = T.at<
double>(2);
299 const std::list<std::pair<
int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
300 std::vector<uchar> & status,
301 double ransacReprojThreshold,
302 double ransacConfidence)
305 status = std::vector<uchar>(pairs.size(), 0);
308 cv::Mat points1(1, pairs.size(), CV_32FC2);
309 cv::Mat points2(1, pairs.size(), CV_32FC2);
311 float * points1data = points1.ptr<
float>(0);
312 float * points2data = points2.ptr<
float>(0);
316 for(std::list<std::pair<
int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::const_iterator iter = pairs.begin();
320 points1data[i*2] = (*iter).second.first.pt.x;
321 points1data[i*2+1] = (*iter).second.first.pt.y;
323 points2data[i*2] = (*iter).second.second.pt.x;
324 points2data[i*2+1] = (*iter).second.second.pt.y;
333 cv::Mat fundamentalMatrix = cv::findFundamentalMat(
338 ransacReprojThreshold,
344 bool fundMatFound =
false;
345 UASSERT(fundamentalMatrix.type() == CV_64FC1);
346 if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
347 (fundamentalMatrix.at<
double>(0,0) != 0.0 ||
348 fundamentalMatrix.at<
double>(0,1) != 0.0 ||
349 fundamentalMatrix.at<
double>(0,2) != 0.0 ||
350 fundamentalMatrix.at<
double>(1,0) != 0.0 ||
351 fundamentalMatrix.at<
double>(1,1) != 0.0 ||
352 fundamentalMatrix.at<
double>(1,2) != 0.0 ||
353 fundamentalMatrix.at<
double>(2,0) != 0.0 ||
354 fundamentalMatrix.at<
double>(2,1) != 0.0 ||
355 fundamentalMatrix.at<
double>(2,2) != 0.0) )
367 "F = [%f %f %f;%f %f %f;%f %f %f]",
368 fundamentalMatrix.ptr<
double>(0)[0],
369 fundamentalMatrix.ptr<
double>(0)[1],
370 fundamentalMatrix.ptr<
double>(0)[2],
371 fundamentalMatrix.ptr<
double>(0)[3],
372 fundamentalMatrix.ptr<
double>(0)[4],
373 fundamentalMatrix.ptr<
double>(0)[5],
374 fundamentalMatrix.ptr<
double>(0)[6],
375 fundamentalMatrix.ptr<
double>(0)[7],
376 fundamentalMatrix.ptr<
double>(0)[8]);
378 return fundamentalMatrix;
386 UASSERT(p.cols == 4 && p.rows == 3);
387 r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
395 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
400 cv::Mat tx = (cv::Mat_<double>(3,3) <<
405 cv::Mat K = (cv::Mat_<double>(3,3) <<
412 return K.inv().t()*E*K.inv();
431 cv::Mat A = (cv::Mat_<double>(4,3) <<
432 u.x*P(2,0)-P(0,0), u.x*P(2,1)-P(0,1), u.x*P(2,2)-P(0,2),
433 u.y*P(2,0)-P(1,0), u.y*P(2,1)-P(1,1), u.y*P(2,2)-P(1,2),
434 u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1), u1.x*P1(2,2)-P1(0,2),
435 u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1), u1.y*P1(2,2)-P1(1,2)
437 cv::Mat B = (cv::Mat_<double>(4,1) <<
438 -(u.x*P(2,3) -P(0,3)),
439 -(u.y*P(2,3) -P(1,3)),
440 -(u1.x*P1(2,3) -P1(0,3)),
441 -(u1.y*P1(2,3) -P1(1,3)));
444 solve(A,B,X,cv::DECOMP_SVD);
456 const cv::Matx34d & P,
458 const cv::Matx34d & P1)
460 double wi = 1, wi1 = 1;
461 double EPSILON = 0.0001;
463 cv::Mat X(4,1,CV_64FC1);
465 X.at<
double>(0) = X_.at<
double>(0);
466 X.at<
double>(1) = X_.at<
double>(1);
467 X.at<
double>(2) = X_.at<
double>(2);
468 X.at<
double>(3) = 1.0;
469 for (
int i=0; i<10; i++)
472 double p2x = cv::Mat(cv::Mat(P).
row(2)*X).at<
double>(0);
473 double p2x1 = cv::Mat(cv::Mat(P1).
row(2)*X).at<
double>(0);
476 if(fabs(wi - p2x) <= EPSILON && fabs(wi1 - p2x1) <= EPSILON)
break;
482 cv::Mat A = (cv::Mat_<double>(4,3) <<
483 (u.x*P(2,0)-P(0,0))/wi, (u.x*P(2,1)-P(0,1))/wi, (u.x*P(2,2)-P(0,2))/wi,
484 (u.y*P(2,0)-P(1,0))/wi, (u.y*P(2,1)-P(1,1))/wi, (u.y*P(2,2)-P(1,2))/wi,
485 (u1.x*P1(2,0)-P1(0,0))/wi1, (u1.x*P1(2,1)-P1(0,1))/wi1, (u1.x*P1(2,2)-P1(0,2))/wi1,
486 (u1.y*P1(2,0)-P1(1,0))/wi1, (u1.y*P1(2,1)-P1(1,1))/wi1, (u1.y*P1(2,2)-P1(1,2))/wi1);
487 cv::Mat B = (cv::Mat_<double>(4,1) <<
488 -(u.x*P(2,3) -P(0,3))/wi,
489 -(u.y*P(2,3) -P(1,3))/wi,
490 -(u1.x*P1(2,3) -P1(0,3))/wi1,
491 -(u1.y*P1(2,3) -P1(1,3))/wi1);
493 solve(A,B,X_,cv::DECOMP_SVD);
494 X.at<
double>(0) = X_.at<
double>(0);
495 X.at<
double>(1) = X_.at<
double>(1);
496 X.at<
double>(2) = X_.at<
double>(2);
497 X.at<
double>(3) = 1.0;
507 const cv::Mat& pt_set,
508 const cv::Mat& pt_set1,
511 pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
512 std::vector<double> & reproj_errors)
514 pointcloud.reset(
new pcl::PointCloud<pcl::PointXYZ>);
516 unsigned int pts_size = pt_set.cols;
518 pointcloud->resize(pts_size);
519 reproj_errors.resize(pts_size);
521 for(
unsigned int i=0; i<pts_size; i++)
523 cv::Point3d u(pt_set.at<
double>(0,i),pt_set.at<
double>(1,i),1.0);
524 cv::Point3d u1(pt_set1.at<
double>(0,i),pt_set1.at<
double>(1,i),1.0);
528 cv::Mat x_proj = P * X;
529 x_proj = x_proj / x_proj.at<
double>(2);
530 cv::Point3d xPt_img_(x_proj.at<
double>(0), x_proj.at<
double>(1), 1.0);
532 double reprj_err = norm(xPt_img_ - u);
533 reproj_errors[i] = reprj_err;
534 pointcloud->at(i) = pcl::PointXYZ(X.at<
double>(0),X.at<
double>(1),X.at<
double>(2));
537 return cv::mean(reproj_errors)[0];
static bool parse(const ParametersMap ¶meters, const std::string &key, bool &value)
static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
int inFrontOfBothCameras(const cv::Mat &x, const cv::Mat &xp, const cv::Mat &R, const cv::Mat &T)
EpipolarGeometry(const ParametersMap ¶meters=ParametersMap())
GLM_FUNC_DECL genType e()
void parseParameters(const ParametersMap ¶meters)
std::map< std::string, std::string > ParametersMap
int _matchCountMinAccepted
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
GLM_FUNC_DECL T determinant(matType< T, P > const &m)
Basic mathematics functions.
const std::vector< cv::KeyPoint > & getWordsKpts() const
virtual ~EpipolarGeometry()
static cv::Mat findFFromWords(const std::list< std::pair< int, std::pair< cv::KeyPoint, cv::KeyPoint > > > &pairs, std::vector< uchar > &status, double ransacReprojThreshold=3.0, double ransacConfidence=0.99)
#define UASSERT(condition)
Wrappers of STL for convenient functions.
static double triangulatePoints(const cv::Mat &pt_set1, const cv::Mat &pt_set2, const cv::Mat &P, const cv::Mat &P1, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud, std::vector< double > &reproj_errors)
const std::multimap< int, int > & getWords() const
#define ULOGGER_DEBUG(...)
static cv::Mat linearLSTriangulation(cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1)
static void findEpipolesFromF(const cv::Mat &fundamentalMatrix, cv::Vec3d &e1, cv::Vec3d &e2)
bool check(const Signature *ssA, const Signature *ssB)
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
static int findPairsUnique(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
ULogger class and convenient macros.
static void findRTFromP(const cv::Mat &p, cv::Mat &r, cv::Mat &t)
T uSum(const std::list< T > &list)
#define ULOGGER_ERROR(...)
static cv::Mat findPFromE(const cv::Mat &E, const cv::Mat &x, const cv::Mat &xp)
static cv::Mat iterativeLinearLSTriangulation(cv::Point3d u, const cv::Matx34d &P, cv::Point3d u1, const cv::Matx34d &P1)