|
@@ -0,0 +1,96 @@
|
|
|
+#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;
|
|
|
+}
|