RcdMathLib_doc
Open Source Library for Linear and Non-linear Algebra
loc_levenberg_marquardt.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 #include <math.h>
25 #include <float.h>
26 #include <inttypes.h>
27 
28 #include "matrix.h"
29 #include "vector.h"
30 #include "solve.h"
31 #include "utils.h"
33 
35  matrix_t ref_points_matrix[ref_points_num][3],
36  matrix_t point[3],
37  matrix_t measured_data_vec[ref_points_num], matrix_t mu,
38  matrix_t s[3],
39  void (*f_i)(uint8_t ref_point_num,
40  matrix_t ref_point_mat[ref_points_num][3],
41  matrix_t point[3], matrix_t d_vec[],
42  matrix_t f_vec[]),
43  void (*jacobian_get_JTJ)(
44  uint8_t ref_points_num,
45  matrix_t ref_point_matrix[ref_points_num][3],
46  matrix_t point[3],
47  matrix_t data_vec[ref_points_num],
48  matrix_t JTJ[3][3]),
49  void (*jacobian_get_JTf)(
50  uint8_t ref_points_num,
51  matrix_t ref_point_matrix[ref_points_num][3],
52  matrix_t point[3],
53  matrix_t data_vec[ref_points_num],
54  matrix_t JTf[3]),
55  void (*jacobian_get_J_mul_s)(
56  uint8_t ref_points_num,
57  matrix_t ref_point_matrix[ref_points_num][3],
58  matrix_t point[3], matrix_t s[3],
59  matrix_t J_s[ref_points_num])
60  )
61 {
62 
63  matrix_t Fx[ref_points_num];
64  matrix_t JTJ_mu2_I[3][3];
65  matrix_t JTF[3];
66  matrix_t ro_mu = 0;
67  matrix_t Fx_square = 0;
68  matrix_t Fx_plus_s_square = 0;
69  matrix_t Fx_plus_J_mul_s_square = 0;
70  matrix_t Fx_plus_J_mul_s[ref_points_num];
71  matrix_t F_x_plus_s[ref_points_num];
72  matrix_t point_plus_s[3];
73  matrix_t denom;
74 
75  //compute: -(J'J + mu^2*I)
76  loc_levenberg_marquardt_get_JTJ_mu2_I(ref_points_num, ref_points_matrix,
77  point,
78  measured_data_vec, mu, JTJ_mu2_I,
79  jacobian_get_JTJ);
80  matrix_mul_scalar(3, 3, JTJ_mu2_I, -1, JTJ_mu2_I); // -(J'*J+mu^2*eye(n)) is an (n,n) matrix
81 
82  jacobian_get_JTf(ref_points_num, ref_points_matrix, point,
83  measured_data_vec, JTF);
84 
85  //solve the equation: s = -(J'*J+mu^2*I)\(J'*Fx);
86  solve_householder(3, 3, JTJ_mu2_I, JTF, s);
87 
88  //f(x)
89  f_i(ref_points_num, ref_points_matrix, point, measured_data_vec, Fx);
90  //f(x)^2
91  Fx_square = vector_get_scalar_product(ref_points_num, Fx, Fx);
92 
93  //(x+s)
94  vector_add(3, point, s, point_plus_s);
95 
96  //f(x+s)
97  f_i(ref_points_num, ref_points_matrix, point_plus_s, measured_data_vec,
98  F_x_plus_s);
99  //f(x+s)^2
100  Fx_plus_s_square = vector_get_scalar_product(ref_points_num, F_x_plus_s,
101  F_x_plus_s);
102 
103  //compute: J(x)*s
104  jacobian_get_J_mul_s(ref_points_num, ref_points_matrix, point, s,
105  Fx_plus_J_mul_s);
106  //compute: f(x) + J(x)*s
107  vector_add(ref_points_num, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);
108 
109  //compute: ||f(x) + J(x)*s||^2
110  Fx_plus_J_mul_s_square = vector_get_scalar_product(ref_points_num,
111  Fx_plus_J_mul_s,
112  Fx_plus_J_mul_s);
113 
114  denom = Fx_square - Fx_plus_J_mul_s_square;
115  if (denom != 0) {
116  ro_mu = (Fx_square - Fx_plus_s_square) / denom;
117  }
118  else {
119  puts("ro_mu is infinite !!!");
120  ro_mu = FLT_MAX;
121  }
122 
123  return ro_mu;
124 }
125 
126 uint8_t loc_levenberg_marquardt(uint8_t ref_points_num,
127  matrix_t ref_points_matrix[ref_points_num][3],
128  matrix_t start_pos[3],
129  matrix_t measured_data_vec[ref_points_num],
130  matrix_t eps, matrix_t tau, matrix_t beta0,
131  matrix_t beta1,
132  uint8_t max_iter_num,
133  matrix_t est_pos[3],
134  void (*f_i)(uint8_t ref_points_num,
135  matrix_t ref_point_mat[ref_points_num][3],
136  matrix_t point[3], matrix_t d_vec[],
137  matrix_t f_vec[]),
138  void (*jacobian_get_JTJ)(uint8_t ref_points_num,
139  matrix_t ref_point_matrix[][3],
140  matrix_t point[3],
141  matrix_t data_vec[ref_points_num],
142  matrix_t JTJ[3][3]),
143  void (*jacobian_get_JTf)(uint8_t ref_points_num,
144  matrix_t ref_point_matrix[][3],
145  matrix_t point[3],
146  matrix_t data_vec[ref_points_num],
147  matrix_t JTf[3]),
148  void (*jacobian_get_J_mul_s)(uint8_t ref_points_num,
149  matrix_t ref_point_matrix[][3],
150  matrix_t point[3], matrix_t s[3],
151  matrix_t J_s[ref_points_num])
152  )
153 {
154 
155  matrix_t JTF[3];
156  matrix_t JTJ_mu2_I[3][3];
157  matrix_t s[3];
158  matrix_t mu;
159  matrix_t ro_mu;
160  uint8_t it;
161 
162  /*
163  * compute mu0 and -(J'J + mu0^2*I)
164  */
165  //store J'J in JTJ_mu2_I
166  jacobian_get_JTJ(ref_points_num, ref_points_matrix, start_pos,
167  measured_data_vec, JTJ_mu2_I);
168  mu = loc_levenberg_marquardt_get_mu0(tau, JTJ_mu2_I);
169 
170  //compute: J'J + mu0^2*I
171  loc_levenberg_marquardt_get_JTJ_mu2_I(ref_points_num, ref_points_matrix,
172  start_pos, measured_data_vec, mu,
173  JTJ_mu2_I,
174  jacobian_get_JTJ);
175  //-(J'*J+mu0^2*eye(n)) is an (n,n) matrix
176  matrix_mul_scalar(3, 3, JTJ_mu2_I, -1, JTJ_mu2_I);
177 
178  //compute JTF
179  jacobian_get_JTf(ref_points_num, ref_points_matrix, start_pos,
180  measured_data_vec, JTF);
181 
182  //solve the equation: s=-(J'*J+mu^2*eye(n))\(J'*F);
183  solve_householder(3, 3, JTJ_mu2_I, JTF, s);
184 
185  vector_copy(3, start_pos, est_pos);
186  it = 0;
187  while ((vector_get_norm2(3, s)
188  > eps * (1 + vector_get_norm2(3, est_pos)))
189  && (it < max_iter_num)) { //norm(s,2)>eps*(1+norm(x,2))
190 
191  ro_mu = loc_levenberg_marquardt_correction(ref_points_num,
192  ref_points_matrix, est_pos,
193  measured_data_vec,
194  mu, s, f_i, jacobian_get_JTJ,
195  jacobian_get_JTf,
196  jacobian_get_J_mul_s);
197 
198  while (1) {
199  if (ro_mu <= beta0) {
200  mu = 2.0 * mu;
202  ref_points_num,
203  ref_points_matrix, est_pos,
204  measured_data_vec, mu, s, f_i,
205  jacobian_get_JTJ,
206  jacobian_get_JTf,
207  jacobian_get_J_mul_s);
208  }
209  else if (ro_mu >= beta1) {
210  mu = mu / 2.0;
211  break;
212  }
213  else {
214  break;
215  }
216  }
217  vector_add(3, est_pos, s, est_pos);
218  it = it + 1;
219  } //while
220  return it;
221 }
222 
224 {
225  matrix_t max_diag_JTJ = JTJ[0][0];
226 
227  for (uint8_t i = 1; i < 3; i++) {
228  if (JTJ[i][i] > max_diag_JTJ) {
229  max_diag_JTJ = JTJ[i][i];
230  }
231  }
232  return tau * max_diag_JTJ;
233 }
234 
235 //compute: J'J + mu^2*I
236 void loc_levenberg_marquardt_get_JTJ_mu2_I(uint8_t ref_points_num,
237  matrix_t ref_points_matrix[ref_points_num][3],
238  matrix_t point[3],
239  matrix_t measured_data_vec[ref_points_num],
240  matrix_t mu,
241  matrix_t JTJ_mu2_I[3][3],
242  void (*jacobian_get_JTJ)(
243  uint8_t ref_points_num,
244  matrix_t ref_point_matrix[ref_points_num][3],
245  matrix_t point[3],
246  matrix_t data_vec[ref_points_num],
247  matrix_t JTJ[3][3])
248  )
249 {
250  uint8_t i;
251 
252  jacobian_get_JTJ(ref_points_num, ref_points_matrix, point,
253  measured_data_vec, JTJ_mu2_I); //JTJ_mu2_I = J'J
254 
255  for (i = 0; i < 3; i++) {
256  JTJ_mu2_I[i][i] += pow(mu, 2); // JTJ_mu2_I = J'*J+mu^2*I;
257  }
258 }
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
vector_get_scalar_product
vector_t vector_get_scalar_product(uint8_t n, vector_t vec1[n], vector_t vec2[n])
Compute the dot product of two vectors.
Definition: vector.c:190
solve_householder
int8_t solve_householder(uint8_t m, uint8_t n, matrix_t A[][n], matrix_t b[m], matrix_t x_sol[n])
Solve an (m n) linear system Ax = b, using the Householder algorithm.
Definition: solve.c:77
loc_levenberg_marquardt.h
Implement the Levenberg–Marquardt (LVM) algorithm for position optimization.
solve.h
Enables to solve systems of linear equations Ax = b for x.
matrix.h
Matrix computations.
utils.h
Utilities for linear algebra.
matrix_mul_scalar
void matrix_mul_scalar(uint8_t m, uint8_t n, matrix_t mat_src[m][n], matrix_t value, matrix_t mat_dest[m][n])
Multiply all elements of a matrix with a specified value.
Definition: matrix.c:602
loc_levenberg_marquardt_get_mu0
matrix_t loc_levenberg_marquardt_get_mu0(matrix_t tau, matrix_t JTJ[3][3])
Compute the initial value of the Levenberg–Marquardt (LVM) algorithm.
Definition: loc_levenberg_marquardt.c:223
loc_levenberg_marquardt
uint8_t loc_levenberg_marquardt(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t start_pos[3], matrix_t measured_data_vec[ref_points_num], matrix_t eps, matrix_t tau, matrix_t beta0, matrix_t beta1, uint8_t max_iter_num, matrix_t est_pos[3], void(*f_i)(uint8_t ref_points_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[][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[][3], matrix_t point[3], matrix_t data_vec[ref_points_num], matrix_t JTf[3]), void(*jacobian_get_J_mul_s)(uint8_t ref_points_num, matrix_t ref_point_matrix[][3], matrix_t point[3], matrix_t s[3], matrix_t J_s[ref_points_num]))
Implements the Levenberg–Marquardt (LVM) algorithm.
Definition: loc_levenberg_marquardt.c:126
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
matrix_t
#define matrix_t
Define the data type of the matrix elements.
Definition: matrix.h:38
vector_add
void vector_add(uint8_t size, vector_t a_vec[size], vector_t b_vec[size], vector_t a_plus_b_vec[size])
Compute the addition of two vectors.
Definition: vector.c:104
vector.h
Vector computations.
loc_levenberg_marquardt_correction
matrix_t loc_levenberg_marquardt_correction(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t point[3], matrix_t measured_data_vec[ref_points_num], matrix_t mu, matrix_t s[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]), void(*jacobian_get_J_mul_s)(uint8_t ref_points_num, matrix_t ref_point_matrix[ref_points_num][3], matrix_t point[3], matrix_t s[3], matrix_t J_s[ref_points_num]))
Implements the correction-function of the Levenberg–Marquardt (LVM) algorithm.
Definition: loc_levenberg_marquardt.c:34
loc_levenberg_marquardt_get_JTJ_mu2_I
void loc_levenberg_marquardt_get_JTJ_mu2_I(uint8_t ref_points_num, matrix_t ref_points_matrix[ref_points_num][3], matrix_t point[3], matrix_t measured_data_vec[ref_points_num], matrix_t mu, matrix_t JTJ_mu2_I[3][3], 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]))
Compute the matrix .
Definition: loc_levenberg_marquardt.c:236