i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added EpipolarGeometry::findFFromCalibratedStereoCameras()

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1971 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-06 04:50:46 +00:00
parent c8203f2119
commit a6078bfad4
2 changed files with 24 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/EpipolarGeometry.h
View File

@@ -82,6 +82,8 @@ public:
cv::Mat & r,
cv::Mat & t);
static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5

22
corelib/src/EpipolarGeometry.cpp
View File

@@ -393,6 +393,28 @@ void EpipolarGeometry::findRTFromP(
UDEBUG("");
}
cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
{
cv::Mat R = cv::Mat::ones(3, 3, CV_64FC1);
double Bx = Tx/-fx;
double By = Ty/-fy;
cv::Mat tx = (cv::Mat_<double> <<
0, 0, By,
0, 0, -Bx,
-By, Bx, 0);
cv::Mat K = (cv::Mat_<double> <<
fx, 0, cx,
0, fy, cy,
0, 0, 0);
cv::Mat E = tx*R;
return K.t().inv()*E*K.inv();
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5