RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
loc_gauss_newton.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de>
3  * 2020 Freie Universität Berlin
4  *
5  * This file is subject to the terms and conditions of the GNU Lesser General
6  * Public License v2.1. See the file LICENSE in the top level directory for more
7  * details.
8  */
9 
23 #include <stdio.h>
24 
25 #include "utils.h"
26 #include "matrix.h"
27 #include "vector.h"
29 
30 uint8_t loc_gauss_newton(uint8_t ref_points_num,
31  matrix_t ref_points_matrix[ref_points_num][3],
32  vector_t start_pos[3],
33  matrix_t measured_data_vec[ref_points_num],
34  matrix_t eps, matrix_t fmin, uint8_t max_iter_num,
35  vector_t est_pos[3],
36  void (*f_i)(uint8_t ref_point_num,
37  matrix_t ref_point_mat[ref_points_num][3],
38  matrix_t point[3], matrix_t d_vec[],
39  matrix_t f_vec[]),
40  void (*jacobian_get_JTJ)(uint8_t ref_points_num,
41  matrix_t ref_point_matrix[ref_points_num][3],
42  matrix_t point[3],
43  matrix_t data_vec[ref_points_num],
44  matrix_t JTJ[3][3]),
45  void (*jacobian_get_JTf)(uint8_t ref_points_num,
46  matrix_t ref_point_matrix[ref_points_num][3],
47  matrix_t point[3],
48  matrix_t data_vec[ref_points_num],
49  matrix_t JTf[3])
50  )
51 {
52 
53  matrix_t JT_J[3][3];
54  matrix_t JT_f[3];
55  matrix_t f_vec[ref_points_num];
56  matrix_t f_error;
57  matrix_t pinv_JTJ_mat[3][3];
58  matrix_t correction_vec[3];
59  matrix_t pos[3];
60  matrix_t next_pos[3];
61  matrix_t max_error, min_error;
62  matrix_t step;
63  uint8_t iter_num;
64 
65  f_i(ref_points_num, ref_points_matrix, start_pos, measured_data_vec,
66  f_vec);
67  f_error = vector_get_norm2(ref_points_num, f_vec);
68  max_error = f_error;
69  min_error = max_error;
70  step = eps;
71 
72  vector_copy(3, start_pos, pos);
73  vector_copy(3, start_pos, est_pos);
74  iter_num = 0;
75  while ((step >= eps) && (iter_num < max_iter_num) && (f_error > fmin)) {
76  /*
77  * Compute then correction terms & next positions
78  */
79 
80  //JTJ
81  jacobian_get_JTJ(ref_points_num, ref_points_matrix, pos,
82  measured_data_vec, JT_J);
83 
84  //JTf
85  jacobian_get_JTf(ref_points_num, ref_points_matrix, pos,
86  measured_data_vec, JT_f);
87 
88  //solve: J'J*s = -J'*f
89  moore_penrose_get_pinv(3, 3, JT_J, pinv_JTJ_mat);
90  //s = (J'J)\J'*f
91  matrix_mul_vec(3, 3, pinv_JTJ_mat, JT_f, correction_vec);
92 
93  // x = x - s
94  vector_sub(3, pos, correction_vec, next_pos);
95 
96  //next step
97  step = vector_get_euclidean_distance(3, pos, next_pos);
98 
99  // pos = next_pos
100  vector_copy(3, next_pos, pos);
101 
102  //error vector
103  f_i(ref_points_num, ref_points_matrix, pos, measured_data_vec,
104  f_vec);
105  f_error = vector_get_norm2(ref_points_num, f_vec);
106 
107  if (min_error > f_error) { //store the position with the minimum error
108  vector_copy(3, pos, est_pos);
109  min_error = f_error; // update min_error
110  }
111 
112  max_error = utils_max(f_error, max_error); // update max_error
113  iter_num++;
114  if ((max_error - min_error) > 10) {
115  break;
116  }
117 
118  } //while
119 
120  return iter_num;
121 }
moore_penrose_get_pinv
int8_t moore_penrose_get_pinv(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t pinv_A[n][m])
Calculate the Moore–Penrose inverse of a rectangular matrix.
Definition: moore_penrose_pseudo_inverse.c:38
vector_get_norm2
vector_t vector_get_norm2(uint8_t length, vector_t arr[])
Compute the 2-norm norm of a vector.
Definition: vector.c:42
matrix_mul_vec
void matrix_mul_vec(uint8_t m, uint8_t n, matrix_t matrix[m][n], matrix_t vec[n], matrix_t dst_arr[m])
Compute the multiplication of a matrix with a column vector.
Definition: matrix.c:434
vector_get_euclidean_distance
vector_t vector_get_euclidean_distance(uint8_t length, vector_t vec1[], vector_t vec2[])
Compute the Euclidean distance between two vectors.
Definition: vector.c:163
vector_t
#define vector_t
Define the data type of the vector elements.
Definition: vector.h:33
matrix.h
Matrix computations.
moore_penrose_pseudo_inverse.h
Moore–Penrose algorithm to compute the pseudo-inverse of a matrix.
utils.h
Utilities for linear algebra.
utils_max
double utils_max(double a, double b)
Returns the greater of two real numbers.
Definition: utils.c:55
vector_copy
void vector_copy(uint8_t size, vector_t src_arr[], vector_t dest_arr[])
Copy the elements of the source vector to the destination vector.
Definition: vector.c:37
loc_gauss_newton
uint8_t loc_gauss_newton(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], vector_t start_pos[3], matrix_t measured_data_vec[ref_points_num], matrix_t eps, matrix_t fmin, uint8_t max_iter_num, vector_t est_pos[3], void(*f_i)(uint8_t ref_point_num, matrix_t ref_point_mat[ref_points_num][3], matrix_t point[3], matrix_t d_vec[], matrix_t f_vec[]), void(*jacobian_get_JTJ)(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTJ[3][3]), void(*jacobian_get_JTf)(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTf[3]))
Implements the modified Gauss–Newton algorithm.
Definition: loc_gauss_newton.c:30
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector.h
Vector computations.
vector_sub
void vector_sub(uint8_t size, vector_t a_vec[], vector_t b_vec[], vector_t a_minus_b[])
Compute the subtraction of two vectors.
Definition: vector.c:94