Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
textons.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016, Hann Woei Ho, Guido de Croon
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
32 #include <stdlib.h>
33 #include <stdio.h>
36 #include "mcu_periph/sys_time.h"
37 #include "generated/airframe.h"
38 
39 float ** **dictionary;
43 
44 #define MAX_N_TEXTONS 255
45 
46 // initial settings:
47 #ifndef TEXTONS_RUN
48 #define TEXTONS_RUN 1
49 #endif
50 PRINT_CONFIG_VAR(TEXTONS_RUN)
51 
52 #ifndef TEXTONS_FPS
53 #define TEXTONS_FPS 30
54 #endif
55 PRINT_CONFIG_VAR(TEXTONS_FPS)
56 
57 #ifndef TEXTONS_LOAD_DICTIONARY
58 #define TEXTONS_LOAD_DICTIONARY 1
59 #endif
60 PRINT_CONFIG_VAR(TEXTONS_LOAD_DICTIONARY)
61 
62 #ifndef TEXTONS_REINITIALIZE_DICTIONARY
63 #define TEXTONS_REINITIALIZE_DICTIONARY 0
64 #endif
65 PRINT_CONFIG_VAR(TEXTONS_REINITIALIZE_DICTIONARY)
66 
67 #ifndef TEXTONS_ALPHA
68 #define TEXTONS_ALPHA 0
69 #endif
70 PRINT_CONFIG_VAR(TEXTONS_ALPHA)
71 
72 #ifndef TEXTONS_N_TEXTONS
73 #define TEXTONS_N_TEXTONS 20
74 #endif
75 PRINT_CONFIG_VAR(TEXTONS_N_TEXTONS)
76 
77 #ifndef TEXTONS_N_SAMPLES
78 #define TEXTONS_N_SAMPLES 250
79 #endif
80 PRINT_CONFIG_VAR(TEXTONS_N_SAMPLES)
81 
82 #ifndef TEXTONS_PATCH_SIZE
83 #define TEXTONS_PATCH_SIZE 6
84 #endif
85 PRINT_CONFIG_VAR(TEXTONS_PATCH_SIZE)
86 
87 #ifndef TEXTONS_N_LEARNING_SAMPLES
88 #define TEXTONS_N_LEARNING_SAMPLES 5000
89 #endif
90 PRINT_CONFIG_VAR(TEXTONS_N_LEARNING_SAMPLES)
91 
92 #ifndef TEXTONS_FULL_SAMPLING
93 #define TEXTONS_FULL_SAMPLING 0
94 #endif
95 PRINT_CONFIG_VAR(TEXTONS_FULL_SAMPLING)
96 
97 #ifndef TEXTONS_BORDER_WIDTH
98 #define TEXTONS_BORDER_WIDTH 0
99 #endif
100 PRINT_CONFIG_VAR(TEXTONS_BORDER_WIDTH)
101 
102 #ifndef TEXTONS_BORDER_HEIGHT
103 #define TEXTONS_BORDER_HEIGHT 0
104 #endif
105 PRINT_CONFIG_VAR(TEXTONS_BORDER_HEIGHT)
106 
107 #ifndef TEXTONS_DICTIONARY_NUMBER
108 #define TEXTONS_DICTIONARY_NUMBER 0
109 #endif
110 PRINT_CONFIG_VAR(TEXTONS_DICTIONARY_NUMBER)
111 
112 #ifndef TEXTONS_DICTIONARY_PATH
113 #define TEXTONS_DICTIONARY_PATH /data/ftp/internal_000
114 #endif
115 
116 struct video_listener *listener = NULL;
117 
130 
131 // status variables
133 float alpha = 0.0;
134 
135 // File pointer for saving the dictionary
136 static FILE *dictionary_logger = NULL;
137 
143 struct image_t *texton_func(struct image_t *img, UNUSED uint8_t p);
144 struct image_t *texton_func(struct image_t *img, UNUSED uint8_t p)
145 {
146  // whether to execute the function:
147  if (!running) { return img; }
148 
149  if (img->buf_size == 0) { return img; }
150 
151  // extract frame from img struct:
152  uint8_t *frame = (uint8_t *)img->buf;
153 
154  // if patch size odd, correct:
155  if (patch_size % 2 == 1) { patch_size++; }
156 
157  // check whether we have to reinitialize the dictionary:
159  // set all vars to trigger a reinitialization and learning phase of the dictionary:
160  dictionary_ready = 0;
162  load_dictionary = 0;
163  learned_samples = 0;
164  alpha_uint = 10;
165  // reset reinitialize_dictionary
167  }
168 
169  // if dictionary not initialized:
170  if (dictionary_ready == 0) {
171  if (load_dictionary == 0) {
172 
173  printf("Learned samples: %d / %d\n", learned_samples, n_learning_samples);
174 
175  // Train the dictionary:
176  DictionaryTrainingYUV(frame, img->w, img->h);
177 
178  // After a number of samples, stop learning:
180  // Save the dictionary:
182  // stop learning:
183  dictionary_ready = 1;
184  // lower learning rate
185  alpha = 0.0;
186  printf("Enough learning!\n");
187  alpha_uint = 0;
188  // set learned samples back to 0
189  learned_samples = 0;
190  }
191  } else {
192  // Load the dictionary:
194  }
195  } else {
196  if (alpha_uint > 0) {
197 
198  // printf("Learning, frame time = %d\n", img->ts.tv_sec * 1000 + img->ts.tv_usec / 1000);
199 
200  DictionaryTrainingYUV(frame, img->w, img->h);
201 
203  // Save the dictionary:
205  // reset learned_samples:
206  learned_samples = 0;
207  }
208  } else {
209  // Extract distributions
210  DistributionExtraction(frame, img->w, img->h);
211  }
212 
213  // printf("N textons = %d\n", n_samples_image);
214  // printf("Entropy texton distribution = %f\n", get_entropy(texton_distribution, n_textons));
215  }
216 
217  return img; // Colorfilter did not make a new image
218 }
219 
228 {
229  int i, j, w, s, texton, c; // iterators
230  int x, y; // image coordinates
231  float error_texton; // distance between an image patch and a texton
232 
233  uint8_t *buf;
234 
235  // ***********************
236  // DICTIONARY LEARNING
237  // ***********************
238 
239  if (!dictionary_initialized) {
240  // **************
241  // INITIALISATION
242  // **************
243 
244  printf("Intializing dictionary!\n");
245 
246  // in the first image, we initialize the textons to random patches in the image
247  for (w = 0; w < n_textons; w++) {
248  // select a coordinate
249  x = rand() % (width - patch_size);
250  y = rand() % (height - patch_size);
251 
252  //printf("(x,y) = (%d,%d), (w,h) = (%d,%d), ps = %d\n", x, y, width, height, patch_size);
253  // take the sample
254  for (i = 0; i < patch_size; i++) {
255  buf = frame + (width * 2 * (i + y)) + 2 * x;
256  for (j = 0; j < patch_size; j++) {
257  // put it in a texton
258  printf("Setting dictionary:\n");
259  // U/V component
260 
261  dictionary[w][i][j][0] = (float) * buf;
262  buf += 1;
263  // Y1/Y2 component
264  dictionary[w][i][j][1] = (float) * buf;
265  buf += 1;
266  printf("Done!\n");
267  }
268  }
269  }
271  } else {
272  // ********
273  // LEARNING
274  // ********
275  printf("Learning!");
276  alpha = ((float) alpha_uint) / 255.0;
277 
278  float *texton_distances, * **patch;
279  texton_distances = (float *)calloc(n_textons, sizeof(float));
280  patch = (float ** *)calloc(patch_size, sizeof(float **));
281 
282  for (i = 0; i < patch_size; i++) {
283  patch[i] = (float **)calloc(patch_size, sizeof(float *));
284  for (j = 0; j < patch_size; j++) {
285  patch[i][j] = (float *)calloc(2, sizeof(float));
286  }
287  }
288 
289  // Extract and learn from n_samples_image per image
290  for (s = 0; s < (int) n_samples_image; s++) {
291  // select a random sample from the image
292  x = rand() % (width - patch_size);
293  y = rand() % (height - patch_size);
294 
295  // reset texton_distances
296  for (texton = 0; texton < n_textons; texton++) {
297  texton_distances[texton] = 0;
298  }
299 
300  // extract sample
301  for (i = 0; i < patch_size; i++) {
302  buf = frame + (width * 2 * (i + y)) + 2 * x;
303  for (j = 0; j < patch_size; j++) {
304  // U/V component
305  patch[i][j][0] = (float) * buf;
306  buf += 1;
307  // Y1/Y2 component
308  patch[i][j][1] = (float) * buf;
309  buf += 1;
310  }
311  }
312 
313  // determine distances to the textons:
314  for (i = 0; i < patch_size; i++) {
315  for (j = 0; j < patch_size; j++) {
316  for (c = 0; c < 2; c++) {
317  // determine the distance to textons
318  for (texton = 0; texton < n_textons; texton++) {
319  texton_distances[texton] += (patch[i][j][c] - dictionary[texton][i][j][c])
320  * (patch[i][j][c] - dictionary[texton][i][j][c]);
321  }
322  }
323  }
324  }
325 
326  // search the closest texton
327  int assignment = 0;
328  float min_dist = texton_distances[0];
329  for (texton = 1; texton < n_textons; texton++) {
330  if (texton_distances[texton] < min_dist) {
331  min_dist = texton_distances[texton];
332  assignment = texton;
333  }
334  }
335 
336  // move the neighbour closer to the input
337  for (i = 0; i < patch_size; i++) {
338  for (j = 0; j < patch_size; j++) {
339  for (c = 0; c < 2; c++) {
340  error_texton = patch[i][j][c] - dictionary[assignment][i][j][c];
341  dictionary[assignment][i][j][c] += (alpha * error_texton);
342  }
343  }
344  }
345 
346  // Augment the number of learned samples:
347  learned_samples++;
348  }
349 
350  // Free the allocated memory:
351  for (i = 0; i < patch_size; i++) {
352  for (j = 0; j < patch_size; j++) {
353  free(patch[i][j]);
354  }
355  free(patch[i]);
356  }
357  free(patch);
358  free(texton_distances);
359  }
360 
361  // Free the buffer
362  buf = NULL;
363  free(buf);
364 }
365 
373 {
374  int i, j, texton, c; // iterators
375  int x, y; // coordinates
376  int n_extracted_textons = 0;
377 
378  uint8_t *buf;
379 
380  // ************************
381  // EXECUTION
382  // ************************
383 
384  // Allocate memory for texton distances and image patch:
385  float *texton_distances, * **patch;
386  texton_distances = (float *)calloc(n_textons, sizeof(float));
387  patch = (float ** *)calloc(patch_size, sizeof(float **));
388  for (i = 0; i < patch_size; i++) {
389  patch[i] = (float **)calloc(patch_size, sizeof(float *));
390  for (j = 0; j < patch_size; j++) {
391  patch[i][j] = (float *)calloc(2, sizeof(float));
392  }
393  }
394 
395  int finished = 0;
396  x = 0;
397  y = 0;
398  while (!finished) {
399  if (!FULL_SAMPLING) {
400  x = border_width + rand() % (width - patch_size - 2 * border_width);
401  y = border_height + rand() % (height - patch_size - 2 * border_height);
402  }
403 
404  // reset texton_distances
405  for (texton = 0; texton < n_textons; texton++) {
406  texton_distances[texton] = 0;
407  }
408 
409  // extract sample
410  for (i = 0; i < patch_size; i++) {
411  buf = frame + (width * 2 * (i + y)) + 2 * x;
412  for (j = 0; j < patch_size; j++) {
413  // U/V component
414  patch[i][j][0] = (float) * buf;
415  buf += 1;
416  // Y1/Y2 component
417  patch[i][j][1] = (float) * buf;
418  buf += 1;
419  }
420  }
421 
422  // determine distances:
423  for (i = 0; i < patch_size; i++) {
424  for (j = 0; j < patch_size; j++) {
425  for (c = 0; c < 2; c++) {
426  // determine the distance to words
427  for (texton = 0; texton < n_textons; texton++) {
428  texton_distances[texton] += (patch[i][j][c] - dictionary[texton][i][j][c])
429  * (patch[i][j][c] - dictionary[texton][i][j][c]);
430  }
431  }
432  }
433  }
434 
435  // determine the nearest neighbour
436  // search the closest centroid
437  int assignment = 0;
438  float min_dist = texton_distances[0];
439  for (texton = 1; texton < n_textons; texton++) {
440  if (texton_distances[texton] < min_dist) {
441  min_dist = texton_distances[texton];
442  assignment = texton;
443  }
444  }
445 
446  // put the assignment in the histogram
447  texton_distribution[assignment]++;
448  n_extracted_textons++;
449 
450  if (!FULL_SAMPLING && n_extracted_textons == (int) n_samples_image) {
451  finished = 1;
452  } else {
453  // FULL_SAMPLING is actually a sampling that covers the image:
454  y += patch_size;
455  // True full sampling would require:
456  // y++;
457 
458  if (y > height - patch_size) {
459  if (!FULL_SAMPLING) {
460  x += patch_size;
461  } else {
462  x++;
463  }
464  y = 0;
465  }
466  if (x > width - patch_size) {
467  finished = 1;
468  }
469  }
470  }
471 
472  // Normalize distribution:
473  if (n_extracted_textons > 0) { // should always be the case
474  for (i = 0; i < n_textons; i++) {
475  texton_distribution[i] = texton_distribution[i] / (float) n_extracted_textons;
476  }
477  }
478  // printf("\n");
479 
480 
481  // free memory:
482  for (i = 0; i < patch_size; i++) {
483  for (j = 0; j < patch_size; j++) {
484  free(patch[i][j]);
485  }
486  free(patch[i]);
487  }
488  free(patch);
489  free(texton_distances);
490 
491  buf = NULL;
492  free(buf);
493 
494 } // EXECUTION
495 
496 
497 
502 {
503  //save a dictionary
504  char filename[512];
505 
506  // Check for available files
507  sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number);
508 
509  dictionary_logger = fopen(filename, "w");
510 
511  if (dictionary_logger == NULL) {
512  printf("Filename: %s\n", filename);
513  perror("Error while opening the file.\n");
514  } else {
515  // (over-)write dictionary
516  for (uint8_t i = 0; i < n_textons; i++) {
517  for (uint8_t j = 0; j < patch_size; j++) {
518  for (uint8_t k = 0; k < patch_size; k++) {
519  fprintf(dictionary_logger, "%f\n", dictionary[i][j][k][0]);
520  fprintf(dictionary_logger, "%f\n", dictionary[i][j][k][1]);
521  }
522  }
523  }
524  fclose(dictionary_logger);
525  }
526 
527 }
528 
533 {
534  char filename[512];
535  sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number);
536 
537  if ((dictionary_logger = fopen(filename, "r"))) {
538  // Load the dictionary:
539  for (int i = 0; i < n_textons; i++) {
540  for (int j = 0; j < patch_size; j++) {
541  for (int k = 0; k < patch_size; k++) {
542  if (fscanf(dictionary_logger, "%f\n", &dictionary[i][j][k][0]) == EOF) { break; }
543  if (fscanf(dictionary_logger, "%f\n", &dictionary[i][j][k][1]) == EOF) { break; }
544  }
545  }
546  }
547 
548  fclose(dictionary_logger);
549  dictionary_ready = 1;
550  } else {
551  // If the given dictionary does not exist, we start learning one:
552  printf("Texton dictionary %d does not exist, we start learning one.\n", dictionary_number);
553  load_dictionary = 0;
554  learned_samples = 0;
556  }
557 }
558 
562 void textons_init(void)
563 {
564  printf("Textons init\n");
565  texton_distribution = (float *)calloc(MAX_N_TEXTONS, sizeof(float));
567  learned_samples = 0;
568  dictionary_ready = 0;
569  dictionary = (float ** **)calloc(MAX_N_TEXTONS, sizeof(float ** *));
570  for (int w = 0; w < MAX_N_TEXTONS; w++) {
571  dictionary[w] = (float ** *) calloc(patch_size, sizeof(float **));
572  for (int i = 0; i < patch_size; i++) {
573  dictionary[w][i] = (float **) calloc(patch_size, sizeof(float *));
574  for (int j = 0; j < patch_size; j++) {
575  dictionary[w][i][j] = (float *) calloc(2, sizeof(float));
576  }
577  }
578  }
579 
580  listener = cv_add_to_device(&TEXTONS_CAMERA, texton_func, TEXTONS_FPS, 0);
581 }
582 
583 void textons_stop(void)
584 {
585  free(texton_distribution);
586  free(dictionary);
587 }
588 
594 float get_entropy(float *p_dist, int D)
595 {
596  float entropy = 0.0f;
597  int i;
598  for (i = 0; i < D; i++) {
599  if (p_dist[i] > 0) {
600  entropy -= p_dist[i] * log2(p_dist[i]);
601  }
602  }
603 
604  return entropy;
605 }
static uint8_t frame[20]
uint8_t last_wp UNUSED
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition: cv.c:46
Computer vision framework for onboard processing.
if(GpsFixValid() &&e_identification_started)
uint32_t buf_size
The buffer size.
Definition: image.h:53
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
Definition: image.h:44
static float p[2][2]
static uint32_t s
Architecture independent timing functions.
void save_texton_dictionary(void)
Save the texton dictionary.
Definition: textons.c:501
struct video_listener * listener
Definition: textons.c:116
void load_texton_dictionary(void)
Load a texton dictionary.
Definition: textons.c:532
#define TEXTONS_FPS
Definition: textons.c:53
float * texton_distribution
Definition: textons.c:42
#define TEXTONS_ALPHA
Definition: textons.c:68
#define TEXTONS_PATCH_SIZE
Definition: textons.c:83
uint8_t patch_size
Definition: textons.c:123
uint32_t n_learning_samples
Definition: textons.c:124
uint8_t load_dictionary
Definition: textons.c:119
uint8_t n_textons
Definition: textons.c:122
#define TEXTONS_DICTIONARY_NUMBER
Definition: textons.c:108
uint8_t FULL_SAMPLING
Definition: textons.c:126
#define TEXTONS_N_TEXTONS
Definition: textons.c:73
void DictionaryTrainingYUV(uint8_t *frame, uint16_t width, uint16_t height)
Function that performs one pass for dictionary training.
Definition: textons.c:227
uint8_t dictionary_ready
Definition: textons.c:132
static FILE * dictionary_logger
Definition: textons.c:136
uint32_t n_samples_image
Definition: textons.c:125
uint32_t learned_samples
Definition: textons.c:40
float get_entropy(float *p_dist, int D)
Function that calculates a base-2 Shannon entropy for a probability distribution.
Definition: textons.c:594
#define TEXTONS_DICTIONARY_PATH
Definition: textons.c:113
uint8_t dictionary_initialized
Definition: textons.c:41
float **** dictionary
Definition: textons.c:39
uint8_t alpha_uint
Definition: textons.c:121
#define MAX_N_TEXTONS
Definition: textons.c:44
#define TEXTONS_LOAD_DICTIONARY
Definition: textons.c:58
uint8_t reinitialize_dictionary
Definition: textons.c:120
float alpha
Definition: textons.c:133
#define TEXTONS_BORDER_HEIGHT
Definition: textons.c:103
void textons_stop(void)
Definition: textons.c:583
void textons_init(void)
Initialize.
Definition: textons.c:562
void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height)
Function that extracts a texton histogram from an image.
Definition: textons.c:372
uint32_t border_width
Definition: textons.c:127
uint32_t border_height
Definition: textons.c:128
#define TEXTONS_N_LEARNING_SAMPLES
Definition: textons.c:88
#define TEXTONS_BORDER_WIDTH
Definition: textons.c:98
uint8_t dictionary_number
Definition: textons.c:129
#define TEXTONS_RUN
Definition: textons.c:48
#define TEXTONS_N_SAMPLES
Definition: textons.c:78
#define TEXTONS_REINITIALIZE_DICTIONARY
Definition: textons.c:63
struct image_t * texton_func(struct image_t *img, UNUSED uint8_t p)
Main texton processing function that first either loads or learns a dictionary and then extracts the ...
Definition: textons.c:144
uint8_t running
Definition: textons.c:118
#define TEXTONS_FULL_SAMPLING
Definition: textons.c:93
Takes an image and represents the texture and colors in the image with a texton histogram.
static float D
Definition: trilateration.c:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98