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) {