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);
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
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);
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++ )
{