int main(void)
{
float a11, a12, a13, a21, a22, a23, a31, a32, a33;
a11= -0.558253; a12 = -0.0461681; a13 = -0.505735;
a21 = -0.411397; a22 = 0.0365854; a23 = 0.199707;
a31 = 0.285389; a32 =-0.313789; a33 = 0.200189;
// printf("Original Matrix:\n");
// printMat3(a11, a12, a13, a21, a22, a23, a31, a32, a33);
float u11, u12, u13, u21, u22, u23, u31, u32, u33;
float s11, s12, s13, s21, s22, s23, s31, s32, s33;
float v11, v12, v13, v21, v22, v23, v31, v32, v33;
clock_t start, end;
start = clock();
for (int i=0; i<1e6; i++)
{
svd(a11, a12, a13, a21, a22, a23, a31, a32, a33,
u11, u12, u13, u21, u22, u23, u31, u32, u33,
s11, s12, s13, s21, s22, s23, s31, s32, s33,
v11, v12, v13, v21, v22, v23, v31, v32, v33);
}
end = clock();
printf("Average SVD takes %f microseconds \n ", 1e6*(double(end - start) / 1e6 / CLOCKS_PER_SEC ) );
printf("U:\n");
printMat3(u11, u12, u13, u21, u22, u23, u31, u32, u33);
printf("S:\n");
printMat3(s11, s12, s13, s21, s22, s23, s31, s32, s33);
printf("V:\n");
printMat3(v11, v12, v13, v21, v22, v23, v31, v32, v33);
float t11, t12, t13, t21, t22, t23, t31, t32, t33;
multAB(u11, u12, u13, u21, u22, u23, u31, u32, u33,
s11, s12, s13, s21, s22, s23, s31, s32, s33,
t11, t12, t13, t21, t22, t23, t31, t32, t33);
float m11, m12, m13, m21, m22, m23, m31, m32, m33;
multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33,
v11, v21, v31, v12, v22, v32, v13, v23, v33,
m11, m12, m13, m21, m22, m23, m31, m32, m33);
printf("USV* : \n");
printMat3(m11, m12, m13, m21, m22, m23, m31, m32, m33);
return 0;
}
void test_linalg3() {
//compare Eigen vs. linalg3.h for 3x3 matrix addition, multiplication
//for timing
int n = (int) 1e9;
timeval t0, t1;
Mat3 A,B,C;
setMat3(1,2,3,4,5,6,7,8,9,A);
addcMat3(A,1,B);
//computation time
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
multMatMat3(A,B,C);
}
gettimeofday(&t1, NULL);
//must print or compiler will optimize out loop
std::cout << "(using linalg3) C=\n";
printMat3(C,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
std::cout << std::endl;
if (1) {
//test Eigen
Eigen::Matrix<Real,3,3> A_;
Eigen::Matrix<Real,3,3> B_;
Eigen::Matrix<Real,3,3> C_;
//.data() returns a pointer to the data array of the matrix
copyVec3ToArray(3, (Vec3*) A, A_.data());
copyVec3ToArray(3, (Vec3*) B, B_.data());
//std::cout << "A=\n" << A_ << std::endl;
//std::cout << "B=\n" << B_ << std::endl;
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
//C_ = A_+B_;
C_ = A_*B_;
}
gettimeofday(&t1, NULL);
std::cout << "(using Eigen) C=\n" << C_ << std::endl;
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
std::cout << std::endl;
}
}
void test_qvelToQdot() {
//constants
const int nf = 5;
const int ns = NUMSTATE(nf);
const int nv = NUMQVEL(nf);
Real statedot[ns];
Real qvel[nv],qvel_cvtback[nv];
VecEuler euler = {DEGTORAD(10),DEGTORAD(20),DEGTORAD(30)};
VecOrient orient;
Mat3 R_body_to_world;
if (WMRSIM_USE_QUATERNION)
eulerToQuat(euler,orient);
else
copyEuler(euler,orient);
orientToRot(orient,R_body_to_world);
for (int i=0; i<nv; i++) {
qvel[i] = i;
}
qvelToQdot(nf,qvel,orient,R_body_to_world,statedot);
qdotToQvel(nf,statedot,orient,R_body_to_world,qvel_cvtback); //convert back to check
//PRINT
std::cout << "orient =\n";
printOrient(orient,-1,-1);
std::cout << std::endl;
std::cout << "R_body_to_world =\n";
printMat3(R_body_to_world,-1,-1);
std::cout << std::endl;
std::cout << "qvel =\n";
printMatReal(nv,1,qvel,-1,-1);
std::cout << std::endl;
std::cout << "statedot =\n";
printMatReal(ns,1,statedot,-1,-1);
std::cout << std::endl;
std::cout << "qvel (converted back) =\n";
printMatReal(nv,1,qvel_cvtback,-1,-1);
std::cout << std::endl;
}
int main()
{
// run a simpe test
glm::mat3 A;
// GLM stores matrices in column-major order so this initialization is the transpose of what we want
A = glm::mat3( -0.558253, -0.0461681, -0.505735,
-0.411397 , 0.0365854 , 0.199707,
0.285389 , -0.313789 , 0.200189);
A = glm::transpose(A);
std::cout << "original matrix" << std::endl;
printMat3(A);
/// 2. Symmetric Eigenanlysis
// normal equations matrix
glm::mat3 S = glm::transpose(A) * A;
// std::cout << "normal equations matrix" << std::endl;
// printMat3(S);
glm::quat qV;
jacobiEigenanalysis(S,qV);
std::cout << "final S (diagonalized) " << std::endl;
printMat3(S);
std::cout << "cumulative rotation quaternion" << std::endl;
printQuat(qV);
glm::mat3 V = glm::toMat3(qV); // normalize qV, convert it to matrix
//std::cout << "final rot matrix V" << std::endl;
//printMat3(V);
glm::mat3 B = A * V; // right-multiply A with V => left multiply for column major
std::cout << "B=AV" << std::endl;
//printMat3(B);
///// 3. Sorting the singular values (find V)
sortSingularValues(B,V);
//std::cout << "sorted B=AV" << std::endl;
//printMat3(B);
std::cout << "sorted V" << std::endl;
printMat3(V);
// // if columns 2-3 swapped, also update quaternion representation (i dont think we need to though since we're not tracking quats)
///// 4. QR factorization using Givens rotations (find U,S from B=AV)
glm::mat3 U;
glm::mat3 Sigma;
QRDecomposition(B,U,Sigma);
std::cout << "U" << std::endl;
printMat3(U);
std::cout << "Sigma" << std::endl;
printMat3(Sigma);
glm::mat3 USV = U * Sigma * glm::transpose(V);
std::cout << "product USV'"<< std::endl;
printMat3(USV);
return 0;
}
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);
}
