void rectify::initializeProjectiveTransform()
{
Matrix3f matPCPT;
matPCPT.m[0][0] = (scrWidth -1)*(scrWidth -1); matPCPT.m[0][1] = (scrWidth -1)*(scrHeight -1); matPCPT.m[0][2] = (scrWidth -1)*2;
matPCPT.m[1][0] = (scrWidth -1)*(scrHeight -1);matPCPT.m[1][1] = (scrHeight -1)*(scrHeight -1); matPCPT.m[1][2] = (scrHeight -1)*2;
matPCPT.m[2][0] = (scrWidth -1)*2; matPCPT.m[2][1] = (scrHeight -1)*2; matPCPT.m[2][2] = 4;
matPCPT = matPCPT * 0.25;
Matrix3f matPPT;
matPPT.m[0][0] = (scrWidth * scrWidth -1); matPPT.m[0][1] = 0; matPPT.m[0][2] = 0;
matPPT.m[1][0] = 0; matPPT.m[1][1] = (scrHeight * scrHeight -1); matPPT.m[1][2] = 0;
matPPT.m[2][0] = 0; matPPT.m[2][1] = 0; matPPT.m[2][2] = 0;
float var = (scrWidth * scrHeight)/12.0;
matPPT = matPPT * var;
vector3d epipoleFirst = global::epipoleA;
Matrix3f epiASymmetric; //take global::epipoleA and create antiSymmetric Matrix: A = -transpose(A)
epiASymmetric.m[0][0] = 0; epiASymmetric.m[0][1] = epipoleFirst.z; epiASymmetric.m[0][2] = -epipoleFirst.y;
epiASymmetric.m[1][0] = -epipoleFirst.z; epiASymmetric.m[1][1] = 0; epiASymmetric.m[1][2] = epipoleFirst.x;
epiASymmetric.m[2][0] = epipoleFirst.y; epiASymmetric.m[2][1] = -epipoleFirst.x; epiASymmetric.m[2][2] = 0;
Matrix3f fundamentalMat = global::fundamentalMatrix;
//fundamentalMat.m[1][1] *= (-1);
Matrix3f rectMatA = epiASymmetric.ReturnTranspose() * matPPT * epiASymmetric;
Matrix3f rectMatB = epiASymmetric.ReturnTranspose() * matPCPT * epiASymmetric;
Matrix3f rectMatADash = fundamentalMat.ReturnTranspose() * matPPT * fundamentalMat;
Matrix3f rectMatBDash = fundamentalMat.ReturnTranspose() * matPCPT * fundamentalMat;
Matrix3f orthoMatD, orthoMatDInv, orthoMatD_Dash, orthoMatDInv_Dash;
singularValueDecomposition(rectMatA, orthoMatD, orthoMatDInv); //rectMatA = orthoMatD' * orthoMatD; ' = transpose
singularValueDecomposition(rectMatADash, orthoMatD_Dash, orthoMatDInv_Dash);
/*
std::cout<<std::endl<<" for rectMatA "<<std::endl;
rectMatA.Display("rectMatA");
orthoMatD.Display("matD");
(orthoMatD.ReturnTranspose() * orthoMatD).Display("MUL");
orthoMatDInv.Display("matDInv");
(orthoMatD * orthoMatDInv).Display("ID");
std::cout<<std::endl<<" for rectMatB "<<std::endl;
rectMatADash.Display("rectMatADash");
orthoMatD_Dash.Display("matDDash");
(orthoMatD_Dash.ReturnTranspose() * orthoMatD_Dash).Display("MUL");
orthoMatDInv_Dash.Display("matDDashInv");
(orthoMatD_Dash * orthoMatDInv_Dash).Display("IDDash");
*/
// to maximize weight matrix
Matrix3f weightMat, weightMatDash;
weightMat = orthoMatDInv.ReturnTranspose() * rectMatB * orthoMatDInv;
weightMatDash = orthoMatDInv_Dash.ReturnTranspose() * rectMatBDash * orthoMatDInv_Dash;
vector3d way, way_dash;
powerMaxEigenVec(weightMat, way);
powerMaxEigenVec(weightMatDash, way_dash);
vector3d zed, zed_dash;
zed = orthoMatDInv * way;
zed_dash = orthoMatDInv_Dash * way_dash;
zed.Normalize();
zed_dash.Normalize();
vector3d Zedd = zed * 0.5 + zed_dash * 0.5;
vector3d dublu, dubluDash;
dublu = epiASymmetric * Zedd;
dubluDash = fundamentalMat * Zedd;
dublu.change(dublu.x/dublu.z, dublu.y/dublu.z, 1.0);
dubluDash.change(dubluDash.x/dubluDash.z, dubluDash.y/dubluDash.z, 1.0);
projectiveTrans.m[0][0] = 1; projectiveTrans.m[0][1] = 0; projectiveTrans.m[0][2] = 0;
projectiveTrans.m[1][0] = 0; projectiveTrans.m[1][1] = 1; projectiveTrans.m[1][2] = 0;
projectiveTrans.m[2][0] = dublu.x; projectiveTrans.m[2][1] = dublu.y; projectiveTrans.m[2][2] = 1;
projectiveTransDash.m[0][0] = 1; projectiveTransDash.m[0][1] = 0; projectiveTransDash.m[0][2] = 0;
projectiveTransDash.m[1][0] = 0; projectiveTransDash.m[1][1] = 1; projectiveTransDash.m[1][2] = 0;
projectiveTransDash.m[2][0] = dubluDash.x; projectiveTransDash.m[2][1] = dubluDash.y; projectiveTransDash.m[2][2] = 1;
(projectiveTrans * global::epipoleA).Display("epiA");
(projectiveTransDash * global::epipoleB).Display("epiB");
projectiveTrans.Display("projT");
projectiveTransDash.Display("projTD");
}
