35 matrix_t ref_points_matrix[ref_points_num][3],
39 void (*f_i)(uint8_t ref_point_num,
40 matrix_t ref_point_mat[ref_points_num][3],
43 void (*jacobian_get_JTJ)(
44 uint8_t ref_points_num,
45 matrix_t ref_point_matrix[ref_points_num][3],
49 void (*jacobian_get_JTf)(
50 uint8_t ref_points_num,
51 matrix_t ref_point_matrix[ref_points_num][3],
55 void (*jacobian_get_J_mul_s)(
56 uint8_t ref_points_num,
57 matrix_t ref_point_matrix[ref_points_num][3],
70 matrix_t Fx_plus_J_mul_s[ref_points_num];
78 measured_data_vec, mu, JTJ_mu2_I,
82 jacobian_get_JTf(ref_points_num, ref_points_matrix, point,
83 measured_data_vec, JTF);
89 f_i(ref_points_num, ref_points_matrix, point, measured_data_vec, Fx);
97 f_i(ref_points_num, ref_points_matrix, point_plus_s, measured_data_vec,
104 jacobian_get_J_mul_s(ref_points_num, ref_points_matrix, point, s,
107 vector_add(ref_points_num, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);
114 denom = Fx_square - Fx_plus_J_mul_s_square;
116 ro_mu = (Fx_square - Fx_plus_s_square) / denom;
119 puts(
"ro_mu is infinite !!!");
127 matrix_t ref_points_matrix[ref_points_num][3],
129 matrix_t measured_data_vec[ref_points_num],
132 uint8_t max_iter_num,
134 void (*f_i)(uint8_t ref_points_num,
135 matrix_t ref_point_mat[ref_points_num][3],
138 void (*jacobian_get_JTJ)(uint8_t ref_points_num,
143 void (*jacobian_get_JTf)(uint8_t ref_points_num,
148 void (*jacobian_get_J_mul_s)(uint8_t ref_points_num,
166 jacobian_get_JTJ(ref_points_num, ref_points_matrix, start_pos,
167 measured_data_vec, JTJ_mu2_I);
172 start_pos, measured_data_vec, mu,
179 jacobian_get_JTf(ref_points_num, ref_points_matrix, start_pos,
180 measured_data_vec, JTF);
189 && (it < max_iter_num)) {
192 ref_points_matrix, est_pos,
194 mu, s, f_i, jacobian_get_JTJ,
196 jacobian_get_J_mul_s);
199 if (ro_mu <= beta0) {
203 ref_points_matrix, est_pos,
204 measured_data_vec, mu, s, f_i,
207 jacobian_get_J_mul_s);
209 else if (ro_mu >= beta1) {
227 for (uint8_t i = 1; i < 3; i++) {
228 if (JTJ[i][i] > max_diag_JTJ) {
229 max_diag_JTJ = JTJ[i][i];
232 return tau * max_diag_JTJ;
237 matrix_t ref_points_matrix[ref_points_num][3],
239 matrix_t measured_data_vec[ref_points_num],
242 void (*jacobian_get_JTJ)(
243 uint8_t ref_points_num,
244 matrix_t ref_point_matrix[ref_points_num][3],
252 jacobian_get_JTJ(ref_points_num, ref_points_matrix, point,
253 measured_data_vec, JTJ_mu2_I);
255 for (i = 0; i < 3; i++) {
256 JTJ_mu2_I[i][i] += pow(mu, 2);