├── README.md ├── svd.h ├── main.c └── svd.c /README.md: -------------------------------------------------------------------------------- 1 | # Robotics_TCP_Calibration 2 | Using 6 points Z/X method to calibrate the Tool Center Point of 6 DoFs robot, including Position and Orientation 3 | -------------------------------------------------------------------------------- /svd.h: -------------------------------------------------------------------------------- 1 | #ifndef _SVD_H_ 2 | #define _SVD_H_ 3 | 4 | #include 5 | #include 6 | #include // required for memcpy() 7 | #include // required for DBL_EPSILON 8 | #include // required for fabs(), sqrt(); 9 | 10 | #define MAX_ITERATION_COUNT 30 // Maximum number of iterations 11 | 12 | // Internally Defined Routines 13 | static void Householders_Reduction_to_Bidiagonal_Form(double* A, int nrows, 14 | int ncols, double* U, double* V, double* diagonal, double* superdiagonal ); 15 | static int Givens_Reduction_to_Diagonal_Form( int nrows, int ncols, 16 | double* U, double* V, double* diagonal, double* superdiagonal ); 17 | static void Sort_by_Decreasing_Singular_Values(int nrows, int ncols, 18 | double* singular_value, double* U, double* V); 19 | int Singular_Value_Decomposition(double* A, int nrows, int ncols, double* U, 20 | double* singular_values, double* V, double* dummy_array); 21 | void Singular_Value_Decomposition_Solve(double* U, double* D, double* V, 22 | double tolerance, int nrows, int ncols, double *B, double* x); 23 | void Singular_Value_Decomposition_Inverse(double* U, double* D, double* V, 24 | double tolerance, int nrows, int ncols, double *Astar); 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | #include"svd.h" 2 | 3 | #define M 9 4 | #define N 3 5 | #define K 6 6 | 7 | typedef struct FK 8 | { 9 | double ori[N][N]; 10 | double pos[N]; 11 | }ForwardKinematics; 12 | 13 | #if(0) 14 | // Panda3 15 | #define a2 0.264 16 | #define a3 0.236 17 | #define d1 0.111 18 | #define d3 0.013 19 | #define d4 0.144 20 | #define d5 0.114 21 | #define d6 0.082 22 | 23 | void Fkine( ForwardKinematics *state, double *point) 24 | { 25 | 26 | // Orientaion 27 | state->ori[0][0] = cos(point[5])*(sin(point[0])*sin(point[4]) + cos(point[1]+point[2]+point[3])*cos(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*cos(point[0])*sin(point[5]); 28 | state->ori[0][1] = -sin(point[5])*(sin(point[0])*sin(point[4]) + cos(point[1]+point[2]+point[3])*cos(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*cos(point[0])*cos(point[5]); 29 | state->ori[0][2] = cos(point[4])*sin(point[0]) - cos(point[1]+point[2]+point[3])*cos(point[0])*sin(point[4]); 30 | 31 | state->ori[1][0] = -cos(point[5])*(cos(point[0])*sin(point[4]) - cos(point[1]+point[2]+point[3])*sin(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*sin(point[0])*sin(point[5]); 32 | state->ori[1][1] = sin(point[5])*(cos(point[0])*sin(point[4]) - cos(point[1]+point[2]+point[3])*sin(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*sin(point[0])*cos(point[5]); 33 | state->ori[1][2] = -cos(point[4])*cos(point[0]) - cos(point[1]+point[2]+point[3])*sin(point[0])*sin(point[4]); 34 | 35 | state->ori[2][0] = cos(point[1]+point[2]+point[3])*sin(point[5]) + sin(point[1]+point[2]+point[3])*cos(point[4])*cos(point[5]); 36 | state->ori[2][1] = -sin(point[1]+point[2]+point[3])*cos(point[4])*sin(point[5]) + cos(point[1]+point[2]+point[3])*cos(point[5]); 37 | state->ori[2][2] = -sin(point[1]+point[2]+point[3])*sin(point[4]); 38 | 39 | // Position 40 | state->pos[0] = -a2*cos(point[0])*cos(point[1]) + d6*(cos(point[4])*sin(point[0]) - cos(point[1]+point[2]+point[3])*cos(point[0])*sin(point[4])) + (d3 + d4)*sin(point[0]) + d5*sin(point[1]+point[2]+point[3])*cos(point[0]) - a3*cos(point[0])*cos(point[1])*cos(point[2]) + a3*cos(point[0])*sin(point[1])*sin(point[2]); 41 | state->pos[1] = -d6*(cos(point[0])*cos(point[4]) + cos(point[1]+point[2]+point[3])*sin(point[0])*sin(point[4])) - (d3 + d4)*cos(point[0]) - a2*cos(point[1])*sin(point[0]) + d5*sin(point[1]+point[2]+point[3])*sin(point[0]) - a3*sin(point[0])*cos(point[1])*cos(point[2]) + a3*sin(point[0])*sin(point[1])*sin(point[2]); 42 | state->pos[2] = d1 - a3*sin(point[1]+point[2]) - a2*sin(point[1]) - d5*(cos(point[1]+point[2])*cos(point[3]) -sin(point[1]+point[2])*sin(point[3])) - d6*sin(point[4])*(cos(point[1] + point[2])*sin(point[3]) + sin(point[1] + point[2])*cos(point[3])); 43 | } 44 | 45 | #else 46 | // UR5 47 | #define a2 0.425 48 | #define a3 0.3922 49 | #define d1 0.1625 50 | #define d4 0.1333 51 | #define d5 0.0997 52 | #define d6 0.0996 53 | 54 | void Fkine( ForwardKinematics *state, double *point) 55 | { 56 | // Orientaion 57 | state->ori[0][0] = cos(point[5])*(sin(point[0])*sin(point[4]) + cos(point[1]+point[2]+point[3])*cos(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*cos(point[0])*sin(point[5]); 58 | state->ori[0][1] = -sin(point[5])*(sin(point[0])*sin(point[4]) + cos(point[1]+point[2]+point[3])*cos(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*cos(point[0])*cos(point[5]); 59 | state->ori[0][2] = cos(point[4])*sin(point[0]) - cos(point[1]+point[2]+point[3])*cos(point[0])*sin(point[4]); 60 | state->ori[1][0] = -cos(point[5])*(cos(point[0])*sin(point[4]) - cos(point[1]+point[2]+point[3])*sin(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*sin(point[0])*sin(point[5]); 61 | state->ori[1][1] = sin(point[5])*(cos(point[0])*sin(point[4]) - cos(point[1]+point[2]+point[3])*sin(point[0])*cos(point[4])) - sin(point[1]+point[2]+point[3])*sin(point[0])*cos(point[5]); 62 | state->ori[1][2] = -cos(point[4])*cos(point[0]) - cos(point[1]+point[2]+point[3])*sin(point[0])*sin(point[4]); 63 | state->ori[2][0] = cos(point[1]+point[2]+point[3])*sin(point[5]) + sin(point[1]+point[2]+point[3])*cos(point[4])*cos(point[5]); 64 | state->ori[2][1] = -sin(point[1]+point[2]+point[3])*cos(point[4])*sin(point[5]) + cos(point[1]+point[2]+point[3])*cos(point[5]); 65 | state->ori[2][2] = -sin(point[1]+point[2]+point[3])*sin(point[4]); 66 | 67 | // Position 68 | state->pos[0] = -a2*cos(point[0])*cos(point[1]) + d6*(cos(point[4])*sin(point[0]) - cos(point[1]+point[2]+point[3])*cos(point[0])*sin(point[4])) + d4*sin(point[0]) + d5*sin(point[1]+point[2]+point[3])*cos(point[0]) - a3*cos(point[0])*cos(point[1])*cos(point[2]) + a3*cos(point[0])*sin(point[1])*sin(point[2]); 69 | state->pos[1] = -d6*(cos(point[0])*cos(point[4]) + cos(point[1]+point[2]+point[3])*sin(point[0])*sin(point[4])) - d4*cos(point[0]) - a2*cos(point[1])*sin(point[0]) + d5*sin(point[1]+point[2]+point[3])*sin(point[0]) - a3*sin(point[0])*cos(point[1])*cos(point[2]) + a3*sin(point[0])*sin(point[1])*sin(point[2]); 70 | state->pos[2] = d1 - a3*sin(point[1]+point[2]) - a2*sin(point[1]) - d5*(cos(point[1]+point[2])*cos(point[3]) -sin(point[1]+point[2])*sin(point[3])) - d6*sin(point[4])*(cos(point[1] + point[2])*sin(point[3]) + sin(point[1] + point[2])*cos(point[3])); 71 | } 72 | #endif 73 | 74 | void SubMatrix(double *dst, double *A, double *B, int row, int col) 75 | { 76 | int i; 77 | int j; 78 | for(i=0; iori, 3, 3); 269 | PrintMatrix((double*)(&state1)->pos, 3, 1); 270 | printf("-----------------------\nRotMat_be2 & PosVec_be2:\n-----------------------\n"); 271 | PrintMatrix((double*)(&state2)->ori, 3, 3); 272 | PrintMatrix((double*)(&state2)->pos, 3, 1); 273 | printf("-----------------------\nRotMat_be3 & PosVec_be3:\n-----------------------\n"); 274 | PrintMatrix((double*)(&state3)->ori, 3, 3); 275 | PrintMatrix((double*)(&state3)->pos, 3, 1); 276 | printf("-----------------------\nRotMat_be4 & PosVec_be4:\n-----------------------\n"); 277 | PrintMatrix((double*)(&state4)->ori, 3, 3); 278 | PrintMatrix((double*)(&state4)->pos, 3, 1); 279 | printf("-----------------------\nRotMat_be5 & PosVec_be5:\n-----------------------\n"); 280 | PrintMatrix((double*)(&state5)->ori, 3, 3); 281 | PrintMatrix((double*)(&state5)->pos, 3, 1); 282 | printf("-----------------------\nRotMat_be6 & PosVec_be6:\n-----------------------\n"); 283 | PrintMatrix((double*)(&state6)->ori, 3, 3); 284 | PrintMatrix((double*)(&state6)->pos, 3, 1); 285 | 286 | // Find Matrix A and B and C and D 287 | double sub_ori1[N][N]; 288 | double sub_ori2[N][N]; 289 | double sub_ori3[N][N]; 290 | 291 | double sub_pos1[N]; 292 | double sub_pos2[N]; 293 | double sub_pos3[N]; 294 | 295 | double sub_pos5[N]; 296 | double sub_pos6[N]; 297 | 298 | SubMatrix((double*)sub_ori1, (double*)(&state1)->ori, (double*)(&state2)->ori, N, N); 299 | SubMatrix((double*)sub_ori2, (double*)(&state2)->ori, (double*)(&state3)->ori, N, N); 300 | SubMatrix((double*)sub_ori3, (double*)(&state3)->ori, (double*)(&state4)->ori, N, N); 301 | 302 | SubVector(sub_pos1, (&state2)->pos, (&state1)->pos, N); 303 | SubVector(sub_pos2, (&state3)->pos, (&state2)->pos, N); 304 | SubVector(sub_pos3, (&state4)->pos, (&state3)->pos, N); 305 | 306 | SubVector(sub_pos5, (&state5)->pos, (&state4)->pos, N); 307 | SubVector(sub_pos6, (&state6)->pos, (&state4)->pos, N); 308 | 309 | // Merge A = [sub_ori1; sub_ori2; sub_ori3] 310 | double A[M][N]; 311 | Merge3Matrix((double*)A, (double*)sub_ori1, (double*)sub_ori2, (double*)sub_ori3, M, N); 312 | // Merge B = [sub_pos1; sub_pos2; sub_pos3] 313 | double B[M]; 314 | Merge3Matrix((double*)B, (double*)sub_pos1, (double*)sub_pos2, (double*)sub_pos3, N, 1); 315 | // Merge C = [delta_x * state4->ori 0 0 0 ; 0 0 0 delta_z * state4->ori]; 316 | double delta_x = Norm(sub_pos5, N); 317 | double delta_z = Norm(sub_pos6, N); 318 | double ori_x[N][N]; 319 | double ori_z[N][N]; 320 | double C[K][K]; 321 | MatrixMulConstant((double*)ori_x, (double*)(&state4)->ori, delta_x, N, N); 322 | MatrixMulConstant((double*)ori_z, (double*)(&state4)->ori, delta_z, N, N); 323 | AddZeroToMatrix((double*)C, (double*)ori_x, (double*)ori_z, N, N); 324 | // Merge D = [sub_pos5; sub_pos6] 325 | double D[K]; 326 | Merge2Vector(D, sub_pos5, sub_pos6, 3); 327 | 328 | // Position Calibration 329 | double U[M][N]; 330 | double V[N][N]; 331 | double singular_values_pos[N]; 332 | double pos_cal[N]; 333 | 334 | // Solve Sigular Values Decompose 335 | SVD(pos_cal, (double*)A, (double*)B, (double*)U, (double*)V, 336 | singular_values_pos, M, N); 337 | printf("---------------------------\nPosition Calibration Result:\n---------------------------\n%f\t%f\t%f\t\n\n",pos_cal[0],pos_cal[1],pos_cal[2]); 338 | 339 | // Orientation Calibration 340 | // Rot = [n_et, o_et, a_et] 341 | double O[K][K]; 342 | double P[K][K]; 343 | double singular_values_ori[K]; 344 | double ori_cal[K]; 345 | double n_et[N]; 346 | double a_et[N]; 347 | double o_et[N]; 348 | // Solve Sigular Values Decompose 349 | SVD(ori_cal, (double*)C, (double*)D, (double*)O, (double*)P, 350 | singular_values_ori, K, K); 351 | DeMergeVector(ori_cal, n_et, a_et, N); 352 | 353 | Cross3dVector(o_et, a_et, n_et); 354 | Cross3dVector(a_et, n_et, o_et); 355 | 356 | printf("------------------------------\nOrientation Calibration Result:\n------------------------------\n"); 357 | printf("%f\t%f\t%f\t\n",n_et[0],o_et[0],a_et[0]); 358 | printf("%f\t%f\t%f\t\n",n_et[1],o_et[1],a_et[1]); 359 | printf("%f\t%f\t%f\t\n\n",n_et[2],o_et[2],a_et[2]); 360 | 361 | } 362 | 363 | -------------------------------------------------------------------------------- /svd.c: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: singular_value_decomposition.c // 3 | // Contents: // 4 | // Singular_Value_Decomposition // 5 | // Singular_Value_Decomposition_Solve // 6 | // Singular_Value_Decomposition_Inverse // 7 | //////////////////////////////////////////////////////////////////////////////// 8 | #include"svd.h" 9 | // #include 10 | // #include 11 | // #include // required for memcpy() 12 | // #include // required for DBL_EPSILON 13 | // #include // required for fabs(), sqrt(); 14 | 15 | // #define MAX_ITERATION_COUNT 30 // Maximum number of iterations 16 | 17 | // // Internally Defined Routines 18 | // static void Householders_Reduction_to_Bidiagonal_Form(double* A, int nrows, 19 | // int ncols, double* U, double* V, double* diagonal, double* superdiagonal ); 20 | // static int Givens_Reduction_to_Diagonal_Form( int nrows, int ncols, 21 | // double* U, double* V, double* diagonal, double* superdiagonal ); 22 | // static void Sort_by_Decreasing_Singular_Values(int nrows, int ncols, 23 | // double* singular_value, double* U, double* V); 24 | 25 | //////////////////////////////////////////////////////////////////////////////// 26 | // int Singular_Value_Decomposition(double* A, int nrows, int ncols, // 27 | // double* U, double* singular_values, double* V, double* dummy_array) // 28 | // // 29 | // Description: // 30 | // This routine decomposes an m x n matrix A, with m >= n, into a product // 31 | // of the three matrices U, D, and V', i.e. A = UDV', where U is an m x n // 32 | // matrix whose columns are orthogonal, D is a n x n diagonal matrix, and // 33 | // V is an n x n orthogonal matrix. V' denotes the transpose of V. If // 34 | // m < n, then the procedure may be used for the matrix A'. The singular // 35 | // values of A are the diagonal elements of the diagonal matrix D and // 36 | // correspond to the positive square roots of the eigenvalues of the // 37 | // matrix A'A. // 38 | // // 39 | // This procedure programmed here is based on the method of Golub and // 40 | // Reinsch as given on pages 134 - 151 of the "Handbook for Automatic // 41 | // Computation vol II - Linear Algebra" edited by Wilkinson and Reinsch // 42 | // and published by Springer-Verlag, 1971. // 43 | // // 44 | // The Golub and Reinsch's method for decomposing the matrix A into the // 45 | // product U, D, and V' is performed in three stages: // 46 | // Stage 1: Decompose A into the product of three matrices U1, B, V1' // 47 | // A = U1 B V1' where B is a bidiagonal matrix, and U1, and V1 are a // 48 | // product of Householder transformations. // 49 | // Stage 2: Use Given' transformations to reduce the bidiagonal matrix // 50 | // B into the product of the three matrices U2, D, V2'. The singular // 51 | // value decomposition is then UDV'where U = U2 U1 and V' = V1' V2'. // 52 | // Stage 3: Sort the matrix D in decreasing order of the singular // 53 | // values and interchange the columns of both U and V to reflect any // 54 | // change in the order of the singular values. // 55 | // // 56 | // After performing the singular value decomposition for A, call // 57 | // Singular_Value_Decomposition to solve the equation Ax = B or call // 58 | // Singular_Value_Decomposition_Inverse to calculate the pseudo-inverse // 59 | // of A. // 60 | // // 61 | // Arguments: // 62 | // double* A // 63 | // On input, the pointer to the first element of the matrix // 64 | // A[nrows][ncols]. The matrix A is unchanged. // 65 | // int nrows // 66 | // The number of rows of the matrix A. // 67 | // int ncols // 68 | // The number of columns of the matrix A. // 69 | // double* U // 70 | // On input, a pointer to a matrix with the same number of rows and // 71 | // columns as the matrix A. On output, the matrix with mutually // 72 | // orthogonal columns which is the left-most factor in the singular // 73 | // value decomposition of A. // 74 | // double* singular_values // 75 | // On input, a pointer to an array dimensioned to same as the number // 76 | // of columns of the matrix A, ncols. On output, the singular values // 77 | // of the matrix A sorted in decreasing order. This array corresponds // 78 | // to the diagonal matrix in the singular value decomposition of A. // 79 | // double* V // 80 | // On input, a pointer to a square matrix with the same number of rows // 81 | // and columns as the columns of the matrix A, i.e. V[ncols][ncols]. // 82 | // On output, the orthogonal matrix whose transpose is the right-most // 83 | // factor in the singular value decomposition of A. // 84 | // double* dummy_array // 85 | // On input, a pointer to an array dimensioned to same as the number // 86 | // of columns of the matrix A, ncols. This array is used to store // 87 | // the super-diagonal elements resulting from the Householder reduction// 88 | // of the matrix A to bidiagonal form. And as an input to the Given's // 89 | // procedure to reduce the bidiagonal form to diagonal form. // 90 | // // 91 | // Return Values: // 92 | // 0 Success // 93 | // -1 Failure - During the Given's reduction of the bidiagonal form to // 94 | // diagonal form the procedure failed to terminate within // 95 | // MAX_ITERATION_COUNT iterations. // 96 | // // 97 | // Example: // 98 | // #define M // 99 | // #define N // 100 | // double A[M][N]; // 101 | // double U[M][N]; // 102 | // double V[N][N]; // 103 | // double singular_values[N]; // 104 | // double* dummy_array; // 105 | // // 106 | // (your code to initialize the matrix A) // 107 | // dummy_array = (double*) malloc(N * sizeof(double)); // 108 | // if (dummy_array == NULL) {printf(" No memory available\n"); exit(0); } // 109 | // // 110 | // err = Singular_Value_Decomposition((double*) A, M, N, (double*) U, // 111 | // singular_values, (double*) V, dummy_array); // 112 | // // 113 | // free(dummy_array); // 114 | // if (err < 0) printf(" Failed to converge\n"); // 115 | // else { printf(" The singular value decomposition of A is \n"); // 116 | // ... // 117 | //////////////////////////////////////////////////////////////////////////////// 118 | // // 119 | int Singular_Value_Decomposition(double* A, int nrows, int ncols, double* U, 120 | double* singular_values, double* V, double* dummy_array) 121 | { 122 | Householders_Reduction_to_Bidiagonal_Form( A, nrows, ncols, U, V, 123 | singular_values, dummy_array); 124 | 125 | if (Givens_Reduction_to_Diagonal_Form( nrows, ncols, U, V, 126 | singular_values, dummy_array ) < 0) return -1; 127 | 128 | Sort_by_Decreasing_Singular_Values(nrows, ncols, singular_values, U, V); 129 | 130 | return 0; 131 | } 132 | 133 | 134 | //////////////////////////////////////////////////////////////////////////////// 135 | // static void Householders_Reduction_to_Bidiagonal_Form(double* A, int nrows,// 136 | // int ncols, double* U, double* V, double* diagonal, double* superdiagonal )// 137 | // // 138 | // Description: // 139 | // This routine decomposes an m x n matrix A, with m >= n, into a product // 140 | // of the three matrices U, B, and V', i.e. A = UBV', where U is an m x n // 141 | // matrix whose columns are orthogonal, B is a n x n bidiagonal matrix, // 142 | // and V is an n x n orthogonal matrix. V' denotes the transpose of V. // 143 | // If m < n, then the procedure may be used for the matrix A'. The // 144 | // // 145 | // The matrix U is the product of Householder transformations which // 146 | // annihilate the subdiagonal components of A while the matrix V is // 147 | // the product of Householder transformations which annihilate the // 148 | // components of A to the right of the superdiagonal. // 149 | // // 150 | // The Householder transformation which leaves invariant the first k-1 // 151 | // elements of the k-th column and annihilates the all the elements below // 152 | // the diagonal element is P = I - (2/u'u)uu', u is an nrows-dimensional // 153 | // vector the first k-1 components of which are zero and the last // 154 | // components agree with the current transformed matrix below the diagonal// 155 | // diagonal, the remaining k-th element is the diagonal element - s, where// 156 | // s = (+/-)sqrt(sum of squares of the elements below the diagonal), the // 157 | // sign is chosen opposite that of the diagonal element. // 158 | // // 159 | // Arguments: // 160 | // double* A // 161 | // On input, the pointer to the first element of the matrix // 162 | // A[nrows][ncols]. The matrix A is unchanged. // 163 | // int nrows // 164 | // The number of rows of the matrix A. // 165 | // int ncols // 166 | // The number of columns of the matrix A. // 167 | // double* U // 168 | // On input, a pointer to a matrix with the same number of rows and // 169 | // columns as the matrix A. On output, the matrix with mutually // 170 | // orthogonal columns which is the left-most factor in the bidiagonal // 171 | // decomposition of A. // 172 | // double* V // 173 | // On input, a pointer to a square matrix with the same number of rows // 174 | // and columns as the columns of the matrix A, i.e. V[ncols][ncols]. // 175 | // On output, the orthogonal matrix whose transpose is the right-most // 176 | // factor in the bidiagonal decomposition of A. // 177 | // double* diagonal // 178 | // On input, a pointer to an array dimensioned to same as the number // 179 | // of columns of the matrix A, ncols. On output, the diagonal of the // 180 | // bidiagonal matrix. // 181 | // double* superdiagonal // 182 | // On input, a pointer to an array dimensioned to same as the number // 183 | // of columns of the matrix A, ncols. On output, the superdiagonal // 184 | // of the bidiagonal matrix. // 185 | // // 186 | // Return Values: // 187 | // The function is of type void and therefore does not return a value. // 188 | // The matrices U, V, and the diagonal and superdiagonal are calculated // 189 | // using the addresses passed in the argument list. // 190 | // // 191 | // Example: // 192 | // #define M // 193 | // #define N // 194 | // double A[M][N]; // 195 | // double U[M][N]; // 196 | // double V[N][N]; // 197 | // double diagonal[N]; // 198 | // double superdiagonal[N]; // 199 | // // 200 | // (your code to initialize the matrix A - Note this routine is not // 201 | // (accessible from outside i.e. it is declared static) // 202 | // // 203 | // Householders_Reduction_to_Bidiagonal_Form((double*) A, nrows, ncols, // 204 | // (double*) U, (double*) V, diagonal, superdiagonal ) // 205 | // // 206 | // free(dummy_array); // 207 | // ... // 208 | //////////////////////////////////////////////////////////////////////////////// 209 | // // 210 | static void Householders_Reduction_to_Bidiagonal_Form(double* A, int nrows, 211 | int ncols, double* U, double* V, double* diagonal, double* superdiagonal ) 212 | { 213 | int i,j,k,ip1; 214 | double s, s2, si, scale; 215 | double dum; 216 | double *pu, *pui, *pv, *pvi; 217 | double half_norm_squared; 218 | 219 | // Copy A to U 220 | 221 | memcpy(U,A, sizeof(double) * nrows * ncols); 222 | 223 | // 224 | 225 | diagonal[0] = 0.0; 226 | s = 0.0; 227 | scale = 0.0; 228 | for ( i = 0, pui = U, ip1 = 1; i < ncols; pui += ncols, i++, ip1++ ) { 229 | superdiagonal[i] = scale * s; 230 | // 231 | // Perform Householder transform on columns. 232 | // 233 | // Calculate the normed squared of the i-th column vector starting at 234 | // row i. 235 | // 236 | for (j = i, pu = pui, scale = 0.0; j < nrows; j++, pu += ncols) 237 | scale += fabs( *(pu + i) ); 238 | 239 | if (scale > 0.0) { 240 | for (j = i, pu = pui, s2 = 0.0; j < nrows; j++, pu += ncols) { 241 | *(pu + i) /= scale; 242 | s2 += *(pu + i) * *(pu + i); 243 | } 244 | // 245 | // 246 | // Chose sign of s which maximizes the norm 247 | // 248 | s = ( *(pui + i) < 0.0 ) ? sqrt(s2) : -sqrt(s2); 249 | // 250 | // Calculate -2/u'u 251 | // 252 | half_norm_squared = *(pui + i) * s - s2; 253 | // 254 | // Transform remaining columns by the Householder transform. 255 | // 256 | *(pui + i) -= s; 257 | 258 | for (j = ip1; j < ncols; j++) { 259 | for (k = i, si = 0.0, pu = pui; k < nrows; k++, pu += ncols) 260 | si += *(pu + i) * *(pu + j); 261 | si /= half_norm_squared; 262 | for (k = i, pu = pui; k < nrows; k++, pu += ncols) { 263 | *(pu + j) += si * *(pu + i); 264 | } 265 | } 266 | } 267 | for (j = i, pu = pui; j < nrows; j++, pu += ncols) *(pu + i) *= scale; 268 | diagonal[i] = s * scale; 269 | // 270 | // Perform Householder transform on rows. 271 | // 272 | // Calculate the normed squared of the i-th row vector starting at 273 | // column i. 274 | // 275 | s = 0.0; 276 | scale = 0.0; 277 | if (i >= nrows || i == (ncols - 1) ) continue; 278 | for (j = ip1; j < ncols; j++) scale += fabs ( *(pui + j) ); 279 | if ( scale > 0.0 ) { 280 | for (j = ip1, s2 = 0.0; j < ncols; j++) { 281 | *(pui + j) /= scale; 282 | s2 += *(pui + j) * *(pui + j); 283 | } 284 | s = ( *(pui + ip1) < 0.0 ) ? sqrt(s2) : -sqrt(s2); 285 | // 286 | // Calculate -2/u'u 287 | // 288 | half_norm_squared = *(pui + ip1) * s - s2; 289 | // 290 | // Transform the rows by the Householder transform. 291 | // 292 | *(pui + ip1) -= s; 293 | for (k = ip1; k < ncols; k++) 294 | superdiagonal[k] = *(pui + k) / half_norm_squared; 295 | if ( i < (nrows - 1) ) { 296 | for (j = ip1, pu = pui + ncols; j < nrows; j++, pu += ncols) { 297 | for (k = ip1, si = 0.0; k < ncols; k++) 298 | si += *(pui + k) * *(pu + k); 299 | for (k = ip1; k < ncols; k++) { 300 | *(pu + k) += si * superdiagonal[k]; 301 | } 302 | } 303 | } 304 | for (k = ip1; k < ncols; k++) *(pui + k) *= scale; 305 | } 306 | } 307 | 308 | // Update V 309 | pui = U + ncols * (ncols - 2); 310 | pvi = V + ncols * (ncols - 1); 311 | *(pvi + ncols - 1) = 1.0; 312 | s = superdiagonal[ncols - 1]; 313 | pvi -= ncols; 314 | for (i = ncols - 2, ip1 = ncols - 1; i >= 0; i--, pui -= ncols, 315 | pvi -= ncols, ip1-- ) { 316 | if ( s != 0.0 ) { 317 | pv = pvi + ncols; 318 | for (j = ip1; j < ncols; j++, pv += ncols) 319 | *(pv + i) = ( *(pui + j) / *(pui + ip1) ) / s; 320 | for (j = ip1; j < ncols; j++) { 321 | si = 0.0; 322 | for (k = ip1, pv = pvi + ncols; k < ncols; k++, pv += ncols) 323 | si += *(pui + k) * *(pv + j); 324 | for (k = ip1, pv = pvi + ncols; k < ncols; k++, pv += ncols) 325 | *(pv + j) += si * *(pv + i); 326 | } 327 | } 328 | pv = pvi + ncols; 329 | for ( j = ip1; j < ncols; j++, pv += ncols ) { 330 | *(pvi + j) = 0.0; 331 | *(pv + i) = 0.0; 332 | } 333 | *(pvi + i) = 1.0; 334 | s = superdiagonal[i]; 335 | } 336 | 337 | // Update U 338 | 339 | pui = U + ncols * (ncols - 1); 340 | for (i = ncols - 1, ip1 = ncols; i >= 0; ip1 = i, i--, pui -= ncols ) { 341 | s = diagonal[i]; 342 | for ( j = ip1; j < ncols; j++) *(pui + j) = 0.0; 343 | if ( s != 0.0 ) { 344 | for (j = ip1; j < ncols; j++) { 345 | si = 0.0; 346 | pu = pui + ncols; 347 | for (k = ip1; k < nrows; k++, pu += ncols) 348 | si += *(pu + i) * *(pu + j); 349 | si = (si / *(pui + i) ) / s; 350 | for (k = i, pu = pui; k < nrows; k++, pu += ncols) 351 | *(pu + j) += si * *(pu + i); 352 | } 353 | for (j = i, pu = pui; j < nrows; j++, pu += ncols){ 354 | *(pu + i) /= s; 355 | } 356 | } 357 | else 358 | for (j = i, pu = pui; j < nrows; j++, pu += ncols) *(pu + i) = 0.0; 359 | *(pui + i) += 1.0; 360 | } 361 | } 362 | 363 | 364 | //////////////////////////////////////////////////////////////////////////////// 365 | // static int Givens_Reduction_to_Diagonal_Form( int nrows, int ncols, // 366 | // double* U, double* V, double* diagonal, double* superdiagonal ) // 367 | // // 368 | // Description: // 369 | // This routine decomposes a bidiagonal matrix given by the arrays // 370 | // diagonal and superdiagonal into a product of three matrices U1, D and // 371 | // V1', the matrix U1 premultiplies U and is returned in U, the matrix // 372 | // V1 premultiplies V and is returned in V. The matrix D is a diagonal // 373 | // matrix and replaces the array diagonal. // 374 | // // 375 | // The method used to annihilate the offdiagonal elements is a variant // 376 | // of the QR transformation. The method consists of applying Givens // 377 | // rotations to the right and the left of the current matrix until // 378 | // the new off-diagonal elements are chased out of the matrix. // 379 | // // 380 | // The process is an iterative process which due to roundoff errors may // 381 | // not converge within a predefined number of iterations. (This should // 382 | // be unusual.) // 383 | // // 384 | // Arguments: // 385 | // int nrows // 386 | // The number of rows of the matrix U. // 387 | // int ncols // 388 | // The number of columns of the matrix U. // 389 | // double* U // 390 | // On input, a pointer to a matrix already initialized to a matrix // 391 | // with mutually orthogonal columns. On output, the matrix with // 392 | // mutually orthogonal columns. // 393 | // double* V // 394 | // On input, a pointer to a square matrix with the same number of rows // 395 | // and columns as the columns of the matrix U, i.e. V[ncols][ncols]. // 396 | // The matrix V is assumed to be initialized to an orthogonal matrix. // 397 | // On output, V is an orthogonal matrix. // 398 | // double* diagonal // 399 | // On input, a pointer to an array of dimension ncols which initially // 400 | // contains the diagonal of the bidiagonal matrix. On output, the // 401 | // it contains the diagonal of the diagonal matrix. // 402 | // double* superdiagonal // 403 | // On input, a pointer to an array of dimension ncols which initially // 404 | // the first component is zero and the successive components form the // 405 | // superdiagonal of the bidiagonal matrix. // 406 | // // 407 | // Return Values: // 408 | // 0 Success // 409 | // -1 Failure - The procedure failed to terminate within // 410 | // MAX_ITERATION_COUNT iterations. // 411 | // // 412 | // Example: // 413 | // #define M // 414 | // #define N // 415 | // double U[M][N]; // 416 | // double V[N][N]; // 417 | // double diagonal[N]; // 418 | // double superdiagonal[N]; // 419 | // int err; // 420 | // // 421 | // (your code to initialize the matrices U, V, diagonal, and ) // 422 | // ( superdiagonal. - Note this routine is not accessible from outside) // 423 | // ( i.e. it is declared static.) // 424 | // // 425 | // err = Givens_Reduction_to_Diagonal_Form( M,N,(double*)U,(double*)V, // 426 | // diagonal, superdiagonal ); // 427 | // if ( err < 0 ) printf("Failed to converge\n"); // 428 | // else { ... } // 429 | // ... // 430 | //////////////////////////////////////////////////////////////////////////////// 431 | // // 432 | static int Givens_Reduction_to_Diagonal_Form( int nrows, int ncols, 433 | double* U, double* V, double* diagonal, double* superdiagonal ) 434 | { 435 | 436 | double epsilon; 437 | double c, s; 438 | double f,g,h; 439 | double x,y,z; 440 | double *pu, *pv; 441 | int i,j,k,m; 442 | int rotation_test; 443 | int iteration_count; 444 | 445 | for (i = 0, x = 0.0; i < ncols; i++) { 446 | y = fabs(diagonal[i]) + fabs(superdiagonal[i]); 447 | if ( x < y ) x = y; 448 | } 449 | epsilon = x * DBL_EPSILON; 450 | for (k = ncols - 1; k >= 0; k--) { 451 | iteration_count = 0; 452 | while(1) { 453 | rotation_test = 1; 454 | for (m = k; m >= 0; m--) { 455 | if (fabs(superdiagonal[m]) <= epsilon) {rotation_test = 0; break;} 456 | if (fabs(diagonal[m-1]) <= epsilon) break; 457 | } 458 | if (rotation_test) { 459 | c = 0.0; 460 | s = 1.0; 461 | for (i = m; i <= k; i++) { 462 | f = s * superdiagonal[i]; 463 | superdiagonal[i] *= c; 464 | if (fabs(f) <= epsilon) break; 465 | g = diagonal[i]; 466 | h = sqrt(f*f + g*g); 467 | diagonal[i] = h; 468 | c = g / h; 469 | s = -f / h; 470 | for (j = 0, pu = U; j < nrows; j++, pu += ncols) { 471 | y = *(pu + m - 1); 472 | z = *(pu + i); 473 | *(pu + m - 1 ) = y * c + z * s; 474 | *(pu + i) = -y * s + z * c; 475 | } 476 | } 477 | } 478 | z = diagonal[k]; 479 | if (m == k ) { 480 | if ( z < 0.0 ) { 481 | diagonal[k] = -z; 482 | for ( j = 0, pv = V; j < ncols; j++, pv += ncols) 483 | *(pv + k) = - *(pv + k); 484 | } 485 | break; 486 | } 487 | else { 488 | if ( iteration_count >= MAX_ITERATION_COUNT ) return -1; 489 | iteration_count++; 490 | x = diagonal[m]; 491 | y = diagonal[k-1]; 492 | g = superdiagonal[k-1]; 493 | h = superdiagonal[k]; 494 | f = ( (y - z) * ( y + z ) + (g - h) * (g + h) )/(2.0 * h * y); 495 | g = sqrt( f * f + 1.0 ); 496 | if ( f < 0.0 ) g = -g; 497 | f = ( (x - z) * (x + z) + h * (y / (f + g) - h) ) / x; 498 | // Next QR Transformtion 499 | c = 1.0; 500 | s = 1.0; 501 | for (i = m + 1; i <= k; i++) { 502 | g = superdiagonal[i]; 503 | y = diagonal[i]; 504 | h = s * g; 505 | g *= c; 506 | z = sqrt( f * f + h * h ); 507 | superdiagonal[i-1] = z; 508 | c = f / z; 509 | s = h / z; 510 | f = x * c + g * s; 511 | g = -x * s + g * c; 512 | h = y * s; 513 | y *= c; 514 | for (j = 0, pv = V; j < ncols; j++, pv += ncols) { 515 | x = *(pv + i - 1); 516 | z = *(pv + i); 517 | *(pv + i - 1) = x * c + z * s; 518 | *(pv + i) = -x * s + z * c; 519 | } 520 | z = sqrt( f * f + h * h ); 521 | diagonal[i - 1] = z; 522 | if (z != 0.0) { 523 | c = f / z; 524 | s = h / z; 525 | } 526 | f = c * g + s * y; 527 | x = -s * g + c * y; 528 | for (j = 0, pu = U; j < nrows; j++, pu += ncols) { 529 | y = *(pu + i - 1); 530 | z = *(pu + i); 531 | *(pu + i - 1) = c * y + s * z; 532 | *(pu + i) = -s * y + c * z; 533 | } 534 | } 535 | superdiagonal[m] = 0.0; 536 | superdiagonal[k] = f; 537 | diagonal[k] = x; 538 | } 539 | } 540 | } 541 | return 0; 542 | } 543 | 544 | 545 | //////////////////////////////////////////////////////////////////////////////// 546 | // static void Sort_by_Decreasing_Singular_Values(int nrows, int ncols, // 547 | // double* singular_values, double* U, double* V) // 548 | // // 549 | // Description: // 550 | // This routine sorts the singular values from largest to smallest // 551 | // singular value and interchanges the columns of U and the columns of V // 552 | // whenever a swap is made. I.e. if the i-th singular value is swapped // 553 | // with the j-th singular value, then the i-th and j-th columns of U are // 554 | // interchanged and the i-th and j-th columns of V are interchanged. // 555 | // // 556 | // Arguments: // 557 | // int nrows // 558 | // The number of rows of the matrix U. // 559 | // int ncols // 560 | // The number of columns of the matrix U. // 561 | // double* singular_values // 562 | // On input, a pointer to the array of singular values. On output, the// 563 | // sorted array of singular values. // 564 | // double* U // 565 | // On input, a pointer to a matrix already initialized to a matrix // 566 | // with mutually orthogonal columns. On output, the matrix with // 567 | // mutually orthogonal possibly permuted columns. // 568 | // double* V // 569 | // On input, a pointer to a square matrix with the same number of rows // 570 | // and columns as the columns of the matrix U, i.e. V[ncols][ncols]. // 571 | // The matrix V is assumed to be initialized to an orthogonal matrix. // 572 | // On output, V is an orthogonal matrix with possibly permuted columns.// 573 | // // 574 | // Return Values: // 575 | // The function is of type void. // 576 | // // 577 | // Example: // 578 | // #define M // 579 | // #define N // 580 | // double U[M][N]; // 581 | // double V[N][N]; // 582 | // double diagonal[N]; // 583 | // // 584 | // (your code to initialize the matrices U, V, and diagonal. ) // 585 | // ( - Note this routine is not accessible from outside) // 586 | // ( i.e. it is declared static.) // 587 | // // 588 | // Sort_by_Decreasing_Singular_Values(nrows, ncols, singular_values, // 589 | // (double*) U, (double*) V); // 590 | // ... // 591 | //////////////////////////////////////////////////////////////////////////////// 592 | // // 593 | static void Sort_by_Decreasing_Singular_Values(int nrows, int ncols, 594 | double* singular_values, double* U, double* V) 595 | { 596 | int i,j,max_index; 597 | double temp; 598 | double *p1, *p2; 599 | 600 | for (i = 0; i < ncols - 1; i++) { 601 | max_index = i; 602 | for (j = i + 1; j < ncols; j++) 603 | if (singular_values[j] > singular_values[max_index] ) 604 | max_index = j; 605 | if (max_index == i) continue; 606 | temp = singular_values[i]; 607 | singular_values[i] = singular_values[max_index]; 608 | singular_values[max_index] = temp; 609 | p1 = U + max_index; 610 | p2 = U + i; 611 | for (j = 0; j < nrows; j++, p1 += ncols, p2 += ncols) { 612 | temp = *p1; 613 | *p1 = *p2; 614 | *p2 = temp; 615 | } 616 | p1 = V + max_index; 617 | p2 = V + i; 618 | for (j = 0; j < ncols; j++, p1 += ncols, p2 += ncols) { 619 | temp = *p1; 620 | *p1 = *p2; 621 | *p2 = temp; 622 | } 623 | } 624 | } 625 | 626 | 627 | //////////////////////////////////////////////////////////////////////////////// 628 | // void Singular_Value_Decomposition_Solve(double* U, double* D, double* V, // 629 | // double tolerance, int nrows, int ncols, double *B, double* x) // 630 | // // 631 | // Description: // 632 | // This routine solves the system of linear equations Ax=B where A =UDV', // 633 | // is the singular value decomposition of A. Given UDV'x=B, then // 634 | // x = V(1/D)U'B, where 1/D is the pseudo-inverse of D, i.e. if D[i] > 0 // 635 | // then (1/D)[i] = 1/D[i] and if D[i] = 0, then (1/D)[i] = 0. Since // 636 | // the singular values are subject to round-off error. A tolerance is // 637 | // given so that if D[i] < tolerance, D[i] is treated as if it is 0. // 638 | // The default tolerance is D[0] * DBL_EPSILON * ncols, if the user // 639 | // specified tolerance is less than the default tolerance, the default // 640 | // tolerance is used. // 641 | // // 642 | // Arguments: // 643 | // double* U // 644 | // A matrix with mutually orthonormal columns. // 645 | // double* D // 646 | // A diagonal matrix with decreasing non-negative diagonal elements. // 647 | // i.e. D[i] > D[j] if i < j and D[i] >= 0 for all i. // 648 | // double* V // 649 | // An orthogonal matrix. // 650 | // double tolerance // 651 | // An lower bound for non-zero singular values (provided tolerance > // 652 | // ncols * DBL_EPSILON * D[0]). // 653 | // int nrows // 654 | // The number of rows of the matrix U and B. // 655 | // int ncols // 656 | // The number of columns of the matrix U. Also the number of rows and // 657 | // columns of the matrices D and V. // 658 | // double* B // 659 | // A pointer to a vector dimensioned as nrows which is the right-hand // 660 | // side of the equation Ax = B where A = UDV'. // 661 | // double* x // 662 | // A pointer to a vector dimensioned as ncols, which is the least // 663 | // squares solution of the equation Ax = B where A = UDV'. // 664 | // // 665 | // Return Values: // 666 | // The function is of type void. // 667 | // // 668 | // Example: // 669 | // #define M // 670 | // #define N // 671 | // #define NB // 672 | // double U[M][N]; // 673 | // double V[N][N]; // 674 | // double D[N]; // 675 | // double B[M]; // 676 | // double x[N]; // 677 | // double tolerance; // 678 | // // 679 | // (your code to initialize the matrices U,D,V,B) // 680 | // // 681 | // Singular_Value_Decomposition_Solve((double*) U, D, (double*) V, // 682 | // tolerance, M, N, B, x, bcols) // 683 | // // 684 | // printf(" The solution of Ax=B is \n"); // 685 | // ... // 686 | //////////////////////////////////////////////////////////////////////////////// 687 | // // 688 | 689 | void Singular_Value_Decomposition_Solve(double* U, double* D, double* V, 690 | double tolerance, int nrows, int ncols, double *B, double* x) 691 | { 692 | int i,j,k; 693 | double *pu, *pv; 694 | double dum; 695 | 696 | dum = DBL_EPSILON * D[0] * (double) ncols; 697 | if (tolerance < dum) tolerance = dum; 698 | 699 | for ( i = 0, pv = V; i < ncols; i++, pv += ncols) { 700 | x[i] = 0.0; 701 | for (j = 0; j < ncols; j++) 702 | if (D[j] > tolerance ) { 703 | for (k = 0, dum = 0.0, pu = U; k < nrows; k++, pu += ncols) 704 | dum += *(pu + j) * B[k]; 705 | x[i] += dum * *(pv + j) / D[j]; 706 | } 707 | } 708 | } 709 | 710 | 711 | //////////////////////////////////////////////////////////////////////////////// 712 | // void Singular_Value_Decomposition_Inverse(double* U, double* D, double* V,// 713 | // double tolerance, int nrows, int ncols, double *Astar) // 714 | // // 715 | // Description: // 716 | // This routine calculates the pseudo-inverse of the matrix A = UDV'. // 717 | // where U, D, V constitute the singular value decomposition of A. // 718 | // Let Astar be the pseudo-inverse then Astar = V(1/D)U', where 1/D is // 719 | // the pseudo-inverse of D, i.e. if D[i] > 0 then (1/D)[i] = 1/D[i] and // 720 | // if D[i] = 0, then (1/D)[i] = 0. Because the singular values are // 721 | // subject to round-off error. A tolerance is given so that if // 722 | // D[i] < tolerance, D[i] is treated as if it were 0. // 723 | // The default tolerance is D[0] * DBL_EPSILON * ncols, assuming that the // 724 | // diagonal matrix of singular values is sorted from largest to smallest, // 725 | // if the user specified tolerance is less than the default tolerance, // 726 | // then the default tolerance is used. // 727 | // // 728 | // Arguments: // 729 | // double* U // 730 | // A matrix with mutually orthonormal columns. // 731 | // double* D // 732 | // A diagonal matrix with decreasing non-negative diagonal elements. // 733 | // i.e. D[i] > D[j] if i < j and D[i] >= 0 for all i. // 734 | // double* V // 735 | // An orthogonal matrix. // 736 | // double tolerance // 737 | // An lower bound for non-zero singular values (provided tolerance > // 738 | // ncols * DBL_EPSILON * D[0]). // 739 | // int nrows // 740 | // The number of rows of the matrix U and B. // 741 | // int ncols // 742 | // The number of columns of the matrix U. Also the number of rows and // 743 | // columns of the matrices D and V. // 744 | // double* Astar // 745 | // On input, a pointer to the first element of an ncols x nrows matrix.// 746 | // On output, the pseudo-inverse of UDV'. // 747 | // // 748 | // Return Values: // 749 | // The function is of type void. // 750 | // // 751 | // Example: // 752 | // #define M // 753 | // #define N // 754 | // double U[M][N]; // 755 | // double V[N][N]; // 756 | // double D[N]; // 757 | // double Astar[N][M]; // 758 | // double tolerance; // 759 | // // 760 | // (your code to initialize the matrices U,D,V) // 761 | // // 762 | // Singular_Value_Decomposition_Inverse((double*) U, D, (double*) V, // 763 | // tolerance, M, N, (double*) Astar); // 764 | // // 765 | // printf(" The pseudo-inverse of A = UDV' is \n"); // 766 | // ... // 767 | //////////////////////////////////////////////////////////////////////////////// 768 | // // 769 | 770 | void Singular_Value_Decomposition_Inverse(double* U, double* D, double* V, 771 | double tolerance, int nrows, int ncols, double *Astar) 772 | { 773 | int i,j,k; 774 | double *pu, *pv, *pa; 775 | double dum; 776 | 777 | dum = DBL_EPSILON * D[0] * (double) ncols; 778 | if (tolerance < dum) tolerance = dum; 779 | for ( i = 0, pv = V, pa = Astar; i < ncols; i++, pv += ncols) 780 | for ( j = 0, pu = U; j < nrows; j++, pa++) 781 | for (k = 0, *pa = 0.0; k < ncols; k++, pu++) 782 | if (D[k] > tolerance) *pa += *(pv + k) * *pu / D[k]; 783 | } 784 | --------------------------------------------------------------------------------