31 matrix_t ref_points_matrix[ref_points_num][3],
33 matrix_t measured_data_vec[ref_points_num],
36 void (*f_i)(uint8_t ref_point_num,
37 matrix_t ref_point_mat[ref_points_num][3],
40 void (*jacobian_get_JTJ)(uint8_t ref_points_num,
41 matrix_t ref_point_matrix[ref_points_num][3],
45 void (*jacobian_get_JTf)(uint8_t ref_points_num,
46 matrix_t ref_point_matrix[ref_points_num][3],
65 f_i(ref_points_num, ref_points_matrix, start_pos, measured_data_vec,
69 min_error = max_error;
75 while ((step >= eps) && (iter_num < max_iter_num) && (f_error > fmin)) {
81 jacobian_get_JTJ(ref_points_num, ref_points_matrix, pos,
82 measured_data_vec, JT_J);
85 jacobian_get_JTf(ref_points_num, ref_points_matrix, pos,
86 measured_data_vec, JT_f);
103 f_i(ref_points_num, ref_points_matrix, pos, measured_data_vec,
107 if (min_error > f_error) {
112 max_error =
utils_max(f_error, max_error);
114 if ((max_error - min_error) > 10) {