|
|
|
@ -2296,7 +2296,7 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, |
|
|
|
|
CvRect* roi1, CvRect* roi2 ) |
|
|
|
|
{ |
|
|
|
|
double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4]; |
|
|
|
|
double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; |
|
|
|
|
double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3], _w3[3]; |
|
|
|
|
cv::Rect_<float> inner1, inner2, outer1, outer2; |
|
|
|
|
|
|
|
|
|
CvMat om = cvMat(3, 1, CV_64F, _om); |
|
|
|
@ -2305,11 +2305,13 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, |
|
|
|
|
CvMat r_r = cvMat(3, 3, CV_64F, _r_r); |
|
|
|
|
CvMat pp = cvMat(3, 4, CV_64F, _pp); |
|
|
|
|
CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
|
|
|
|
|
CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps
|
|
|
|
|
CvMat wR = cvMat(3, 3, CV_64F, _wr); |
|
|
|
|
CvMat Z = cvMat(3, 1, CV_64F, _z); |
|
|
|
|
CvMat Ri = cvMat(3, 3, CV_64F, _ri); |
|
|
|
|
double nx = imageSize.width, ny = imageSize.height; |
|
|
|
|
int i, k; |
|
|
|
|
double nt, nw; |
|
|
|
|
|
|
|
|
|
if( matR->rows == 3 && matR->cols == 3 ) |
|
|
|
|
cvRodrigues2(matR, &om); // get vector rotation
|
|
|
|
@ -2320,15 +2322,33 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, |
|
|
|
|
cvMatMul(&r_r, matT, &t); |
|
|
|
|
|
|
|
|
|
int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; |
|
|
|
|
double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); |
|
|
|
|
_uu[idx] = c > 0 ? 1 : -1; |
|
|
|
|
|
|
|
|
|
// calculate global Z rotation
|
|
|
|
|
cvCrossProduct(&t,&uu,&ww); |
|
|
|
|
double nw = cvNorm(&ww, 0, CV_L2); |
|
|
|
|
if (nw > 0.0) |
|
|
|
|
cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); |
|
|
|
|
cvRodrigues2(&ww, &wR); |
|
|
|
|
// if idx == 0
|
|
|
|
|
// e1 = T / ||T||
|
|
|
|
|
// e2 = e1 x [0,0,1]
|
|
|
|
|
|
|
|
|
|
// if idx == 1
|
|
|
|
|
// e2 = T / ||T||
|
|
|
|
|
// e1 = e2 x [0,0,1]
|
|
|
|
|
|
|
|
|
|
// e3 = e1 x e2
|
|
|
|
|
|
|
|
|
|
_uu[2] = 1; |
|
|
|
|
cvCrossProduct(&uu, &t, &ww); |
|
|
|
|
nt = cvNorm(&t, 0, CV_L2); |
|
|
|
|
nw = cvNorm(&ww, 0, CV_L2); |
|
|
|
|
cvConvertScale(&ww, &ww, 1 / nw); |
|
|
|
|
cvCrossProduct(&t, &ww, &w3); |
|
|
|
|
nw = cvNorm(&w3, 0, CV_L2); |
|
|
|
|
cvConvertScale(&w3, &w3, 1 / nw); |
|
|
|
|
_uu[2] = 0; |
|
|
|
|
|
|
|
|
|
for (i = 0; i < 3; ++i) |
|
|
|
|
{ |
|
|
|
|
_wr[idx][i] = -_t[i] / nt; |
|
|
|
|
_wr[idx ^ 1][i] = -_ww[i]; |
|
|
|
|
_wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// apply to both views
|
|
|
|
|
cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); |
|
|
|
@ -2342,16 +2362,11 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, |
|
|
|
|
double fc_new = DBL_MAX; |
|
|
|
|
CvPoint2D64f cc_new[2] = {{0,0}, {0,0}}; |
|
|
|
|
|
|
|
|
|
for( k = 0; k < 2; k++ ) { |
|
|
|
|
const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2; |
|
|
|
|
const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2; |
|
|
|
|
double dk1 = Dk && Dk->data.ptr ? cvmGet(Dk, 0, 0) : 0; |
|
|
|
|
double fc = cvmGet(A,idx^1,idx^1); |
|
|
|
|
if( dk1 < 0 ) { |
|
|
|
|
fc *= 1 + dk1*(nx*nx + ny*ny)/(4*fc*fc); |
|
|
|
|
} |
|
|
|
|
fc_new = MIN(fc_new, fc); |
|
|
|
|
} |
|
|
|
|
newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize; |
|
|
|
|
const double ratio_x = (double)newImgSize.width / imageSize.width / 2; |
|
|
|
|
const double ratio_y = (double)newImgSize.height / imageSize.height / 2; |
|
|
|
|
const double ratio = idx == 1 ? ratio_x : ratio_y; |
|
|
|
|
fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio; |
|
|
|
|
|
|
|
|
|
for( k = 0; k < 2; k++ ) |
|
|
|
|
{ |
|
|
|
|