97 lines
1.8 KiB
C++
Executable File
97 lines
1.8 KiB
C++
Executable File
#include <igl/svd.h>
|
|
#include <cstdlib>
|
|
#include <Accelerate/Accelerate.h>
|
|
#include <cstdio>
|
|
|
|
/* Auxiliary routines prototypes */
|
|
extern void print_matrix( char* desc, int m, int n, double* a, int lda );
|
|
|
|
/* Parameters */
|
|
|
|
void print3x3(const char * s, double * a)
|
|
{
|
|
printf("%s =\n",s);
|
|
for(int i = 0;i<3;i++)
|
|
{
|
|
for(int j = 0;j<3;j++)
|
|
{
|
|
printf("%g ",a[j*3+i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
int main(int argc, char * argv[])
|
|
{
|
|
//// List of rest positions
|
|
//// (0,1)
|
|
//// / \
|
|
//// / \
|
|
//// / \
|
|
//// / \
|
|
//// (-1,0)-----(1,0)
|
|
////
|
|
//double rest[3][3] = {
|
|
// {-1,0,0},
|
|
// {1,0,0},
|
|
// {0,1,0}};
|
|
//// List of pose positions
|
|
////
|
|
//// (0,1)
|
|
//// | \
|
|
//// | \
|
|
//// | (1,0)
|
|
//// | /
|
|
//// | /
|
|
//// (0,-1)
|
|
//double pose[3][3] = {
|
|
// {0,1,0},
|
|
// {0,-1,0},
|
|
// {1,0,0}};
|
|
//// Compute covariance matrix C
|
|
//double C[3*3];
|
|
//// Initialize to zero
|
|
//for(int i = 0;i<3*3;i++)
|
|
//{
|
|
// C[i] = 0;
|
|
//}
|
|
|
|
//// Loop over vertices
|
|
//for(int i = 0;i<3;i++)
|
|
//{
|
|
// // Compute outer product rest[i] * pose[i]
|
|
// // Loop over coordinates
|
|
// for(int j = 0;j<3;j++)
|
|
// {
|
|
// // Loop over coordinates
|
|
// for(int k = 0;k<3;k++)
|
|
// {
|
|
// C[k*3+j] = rest[i][j] * pose[i][k];
|
|
// }
|
|
// }
|
|
//}
|
|
//print3x3("C",C);
|
|
|
|
|
|
//
|
|
//double C[3*3] = {8,3,4,1,5,9,6,7,2};
|
|
double C[3*3] = {5242.55,3364,-0,-8170.15,-5242.56,0,-0,-0,0};
|
|
double u[3*3],s[3],vt[3*3];
|
|
print3x3("C",C);
|
|
// Compute SVD of C
|
|
igl::svd3x3(C,u,s,vt);
|
|
print3x3("u",u);
|
|
print3x3("vt",vt);
|
|
|
|
// Compute R = u*vt
|
|
double R[3*3];
|
|
const double _3 = 3;
|
|
const double _1 = 1;
|
|
cblas_dgemm(CblasColMajor, CblasNoTrans,CblasNoTrans,3,3,3,1,u,3,vt,3,1,R,3);
|
|
print3x3("RT (transposed to be row-major)",R);
|
|
|
|
|
|
return 0;
|
|
}
|