//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// EssentialUtils
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
cv::Point3f EssentialUtils::EpipoleFromEssential(const cv::Matx33f &E)
{
cv::Vec3f Ew;
cv::Matx33f Eu,Evt;
cv::SVD::compute(E, Ew, Eu, Evt);
return cv::Point3f(Eu(0,2),Eu(1,2),Eu(2,2));
}
void ProgValidationTiltPairs::run()
{
MetaData MD_tilted, MD_untilted, DF1sorted, DF2sorted, DFweights;
MD_tilted.read(fntiltimage_In);
MD_untilted.read(fnuntiltimage_In);
DF1sorted.sort(MD_tilted,MDL_ITEM_ID,true);
DF2sorted.sort(MD_untilted,MDL_ITEM_ID,true);
MDIterator iter1(DF1sorted), iter2(DF2sorted);
std::vector< Matrix1D<double> > ang1, ang2;
Matrix1D<double> rotTiltPsi(3), z(3);
size_t currentId;
bool anotherImageIn2=iter2.hasNext();
size_t id1, id2;
bool mirror;
Matrix2D<double> Eu, Et, R;
double alpha, beta;
while (anotherImageIn2)
{
ang1.clear();
ang2.clear();
// Take current id
DF2sorted.getValue(MDL_ITEM_ID,currentId,iter2.objId);
// Grab all the angles in DF2 associated to this id
bool anotherIteration=false;
do
{
DF2sorted.getValue(MDL_ITEM_ID,id2,iter2.objId);
anotherIteration=false;
if (id2==currentId)
{
DF2sorted.getValue(MDL_ANGLE_ROT,XX(rotTiltPsi),iter2.objId);
DF2sorted.getValue(MDL_ANGLE_TILT,YY(rotTiltPsi),iter2.objId);
DF2sorted.getValue(MDL_ANGLE_PSI,ZZ(rotTiltPsi),iter2.objId);
DF2sorted.getValue(MDL_FLIP,mirror,iter2.objId);
std::cout << "From DF2:" << XX(rotTiltPsi) << " " << YY(rotTiltPsi) << " " << ZZ(rotTiltPsi) << " " << mirror << std::endl;
//LINEA ANTERIOR ORIGINAL
if (mirror)
{
double rotp, tiltp, psip;
Euler_mirrorX(XX(rotTiltPsi),YY(rotTiltPsi),ZZ(rotTiltPsi), rotp, tiltp, psip);
XX(rotTiltPsi)=rotp;
YY(rotTiltPsi)=tiltp;
ZZ(rotTiltPsi)=psip;
}
ang2.push_back(rotTiltPsi);
iter2.moveNext();
if (iter2.hasNext())
anotherIteration=true;
}
} while (anotherIteration);
// Advance Iter 1 to catch Iter 2
double N=0, cumulatedDistance=0;
size_t newObjId=0;
if (iter1.objId>0)
{
DF1sorted.getValue(MDL_ITEM_ID,id1,iter1.objId);
while (id1<currentId && iter1.hasNext())
{
iter1.moveNext();
DF1sorted.getValue(MDL_ITEM_ID,id1,iter1.objId);
}
// If we are at the end of DF1, then we did not find id1 such that id1==currentId
if (!iter1.hasNext())
break;
// Grab all the angles in DF1 associated to this id
anotherIteration=false;
do
{
DF1sorted.getValue(MDL_ITEM_ID,id1,iter1.objId);
anotherIteration=false;
if (id1==currentId)
{
DF1sorted.getValue(MDL_ANGLE_ROT,XX(rotTiltPsi),iter1.objId);
DF1sorted.getValue(MDL_ANGLE_TILT,YY(rotTiltPsi),iter1.objId);
DF1sorted.getValue(MDL_ANGLE_PSI,ZZ(rotTiltPsi),iter1.objId);
DF1sorted.getValue(MDL_FLIP,mirror,iter1.objId);
std::cout << "From DF1:" << XX(rotTiltPsi) << " " << YY(rotTiltPsi) << " " << ZZ(rotTiltPsi) << " " << mirror << std::endl;
//LINEA ANTERIOR ORIGINAL
if (mirror)
{
double rotp, tiltp, psip;
Euler_mirrorX(XX(rotTiltPsi),YY(rotTiltPsi),ZZ(rotTiltPsi), rotp, tiltp, psip);
XX(rotTiltPsi)=rotp;
YY(rotTiltPsi)=tiltp;
ZZ(rotTiltPsi)=psip;
}
ang1.push_back(rotTiltPsi);
iter1.moveNext();
if (iter1.hasNext())
anotherIteration=true;
}
} while (anotherIteration);
// Process both sets of angles
for (size_t i=0; i<ang2.size(); ++i)
{
const Matrix1D<double> &anglesi=ang2[i];
double rotu=XX(anglesi);
double tiltu=YY(anglesi);
double psiu=ZZ(anglesi);
Euler_angles2matrix(rotu,tiltu,psiu,Eu,false);
/*std::cout << "------UNTILTED MATRIX------" << std::endl;
std::cout << Eu << std::endl;
std::cout << "vector" << std::endl;
std::cout << Eu(2,0) << " " << Eu(2,1) << " " << Eu(2,2) << std::endl;*/
for (size_t j=0; j<ang1.size(); ++j)
{
const Matrix1D<double> &anglesj=ang1[j];
double rott=XX(anglesj);
double tiltt=YY(anglesj);
double psit=ZZ(anglesj);
double alpha_x, alpha_y;
Euler_angles2matrix(rott,tiltt,psit,Et,false);
//////////////////////////////////////////////////////////////////
double untilt_angles[3]={rotu, tiltu, psiu}, tilt_angles[3]={rott, tiltt, psit};
angles2tranformation(untilt_angles, tilt_angles, alpha_x, alpha_y);
//std::cout << "alpha = " << (alpha_x*alpha_x+alpha_y*alpha_y) << std::endl;
//////////////////////////////////////////////////////////////////
/*std::cout << "------TILTED MATRIX------" << std::endl;
std::cout << Et << std::endl;
std::cout << "vector" << std::endl;
std::cout << Et(2,0) << " " << Et(2,1) << " " << Et(2,2) << std::endl;
std::cout << "---------------------------" << std::endl;
std::cout << "---------------------------" << std::endl;*/
R=Eu*Et.transpose();
double rotTransf, tiltTransf, psiTransf;
Euler_matrix2angles(R, rotTransf, tiltTransf, psiTransf);
std::cout << "Rot_and_Tilt " << rotTransf << " " << tiltTransf << std::endl;
//LINEA ANTERIOR ORIGINAL
XX(z) = Eu(2,0) - Et(2,0);
YY(z) = Eu(2,1) - Et(2,1);
ZZ(z) = Eu(2,2) - Et(2,2);
alpha = atan2(YY(z), XX(z)); //Expressed in rad
beta = atan2(XX(z)/cos(alpha), ZZ(z)); //Expressed in rad
std::cout << "alpha = " << alpha*180/PI << std::endl;
std::cout << "beta = " << beta*180/PI << std::endl;
}
}
}
else
N=0;
if (N>0)
{
double meanDistance=cumulatedDistance/ang2.size();
DFweights.setValue(MDL_ANGLE_DIFF,meanDistance,newObjId);
}
else
if (newObjId>0)
DFweights.setValue(MDL_ANGLE_DIFF,-1.0,newObjId);
anotherImageIn2=iter2.hasNext();
}
std::complex<double> qu[4], qt[4], M[4], Inv_qu[4], test[4], P1[4], P2[4], Inv_quu[4];
double rotu=34*PI/180, tiltu=10*PI/180, psiu=5*PI/180;
double rott=25*PI/180, tiltt=15*PI/180, psit=40*PI/180;
quaternion2Paulibasis(rotu, tiltu, psiu, qu);
/*std::cout << "quaternion2Pauli" << std::endl;
std::cout << "Untilted " << qu[0] << " " << qu[1] << " " << qu[2] << " " << qu[3] << std::endl;
std::cout << " " << std::endl;*/
Paulibasis2matrix(qu,M);
/*std::cout << "Pauli2matrix" << std::endl;
std::cout << "Matriz " << M[0] << " " << M[1] << " " << M[2] << " " << M[3] << std::endl;
std::cout << " " << std::endl;*/
inverse_matrixSU2(M, Inv_qu);
/*std::cout << "inverse_matrixSU(2)" << std::endl;
std::cout << "Inversa " << Inv_qu[0] << " " << Inv_qu[1] << " " << Inv_qu[2] << " " << Inv_qu[3] << std::endl;
std::cout << " " << std::endl;*/
quaternion2Paulibasis(rott, tiltt, psit, qt);
/*std::cout << "quaternion2Pauli" << std::endl;
std::cout << "Tilted " << qt[0] << " " << qt[1] << " " << qt[2] << " " << qt[3] << std::endl;
std::cout << " " << std::endl;*/
InversefromPaulibasis(qu,Inv_quu);
Pauliproduct(qt, Inv_qu, P1);
/*std::cout << "Pauliproduct" << std::endl;
std::cout << "quaternion qt " << P1[0] << " " << P1[1] << " " << P1[2] << " " << P1[3] << std::endl;
std::cout << " " << std::endl;
std::cout << "-----------------------------------" << std::endl;*/
//double alpha_x, alpha_y;
//extrarotationangles(P1, alpha_x, alpha_y);
//std::cout << "alpha_x = " << alpha_x << " " << "alpha_y = " << alpha_y << std::endl;
}
