ElMatrix<double> OriMatrixConvertion(CamStenope * aCS)
{
ElMatrix<double> RotMM2CV(4,3,0.0); //External orientation matrix
ElMatrix<double> P(4,3,0.0); //Orientation (int & ext) matrix in the CV system
ElMatrix<double> F(3,3,0.0); //Internal orientation matrix
ElMatrix<double> Rot(3,3,0.0); //Rotation matrix between photogrammetry and CV systems
ElMatrix<double> Rotx(3,3,0.0); //180° rotation matrix along the x axis
Rotx(0,0)=1;
Rotx(1,1)=-1;
Rotx(2,2)=-1;
Rot=Rotx*aCS->Orient().Mat();
for(int i=0;i<3;i++)
{
RotMM2CV(i,i)=1;
}
RotMM2CV(3,0)=-aCS->VraiOpticalCenter().x;
RotMM2CV(3,1)=-aCS->VraiOpticalCenter().y;
RotMM2CV(3,2)=-aCS->VraiOpticalCenter().z;
RotMM2CV=Rot*RotMM2CV;
F(0,0)=-aCS->Focale();
F(1,1)=aCS->Focale();
Pt2dr PPOut=aCS->DistDirecte(aCS->PP());
F(2,0)=PPOut.x;
F(2,1)=PPOut.y;
F(2,2)=1;
//Computation of the orientation matrix (P=-F*RotMM2CV)
P.mul(F*RotMM2CV,-1);
return P;
}
void test_transform() {
VecEuler euler;
Vec3 translation;
HomogeneousTransform HT,HT2;
setEuler(DEGTORAD(10),DEGTORAD(20),DEGTORAD(30),euler);
setVec3(1,2,3,translation);
poseToHT(euler,translation,HT);
std::cout << "HT=\n"; printHT(HT,-1,-1);
//test rotation matrix
Mat3 Rx,Ry,Rz,Rot,Tmp;
Rotx(euler[0],Rx);
Roty(euler[1],Ry);
Rotz(euler[2],Rz);
multMatMat3(Ry,Rx,Tmp);
multMatMat3(Rz,Tmp,Rot);
std::cout << "Rotz(yaw)*Roty(pit)*Rotx(rol)=\n"; printMat3(Rot,-1,-1);
//test invert transform
invertHT(HT,HT2);
std::cout << "inv(HT)=\n"; printHT(HT2,-1,-1);
//test compose HT
if (1) {
//time it
int n = (int) 1e8;
timeval t0, t1;
//compose Homogeneous Transforms
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
composeHT(HT,HT,HT2);
}
gettimeofday(&t1, NULL);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
} else {
composeHT(HT,HT,HT2);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
}
composeInvHT(HT,HT,HT2);
std::cout << "inv(HT)*HT=\n"; printHT(HT2,-1,-1);
//test transform point
Vec3 p,q;
setVec3(2,3,4,p);
std::cout << "p=\n"; printVec3(p,-1,-1);
applyHT(HT,p,q);
std::cout << "q=HT*p=\n"; printVec3(q,-1,-1);
applyInvHT(HT,p,q);
std::cout << "q=inv(HT)*p=\n"; printVec3(q,-1,-1);
//test converting between angular velocity and Euler rates
Vec3 vel={1,2,3};
VecEuler eulerrate;
MatEuler T;
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
std::cout << "T_vel_to_eulerrate=\n"; printMatReal(3,3,T,-1,-1);
std::cout << "convert back:\n";
eulerrateToVel(euler,eulerrate,vel,0);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << std::endl;
//test at singularity
setEuler(0,M_PI/2,0,euler);
setVec3(0,0,1,vel);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "test at singularity:\n";
std::cout << "euler=\n"; printEuler(euler,-1,-1);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
}
