Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
imavmarker.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "imavmarker.h"
29 
30 
39 #define Img(X,Y)(source[(Y)*input->w*2+(X)*2+1])
40 #define Out(X,Y)(source[(Y)*input->w*2+(X)*2])
41 
42 inline int AbsDiff(int A, int B);
43 inline int AbsDiff(int A, int B)
44 {
45  if (A < B) {
46  return B - A;
47  }
48  return A - B;
49 }
50 
51 struct marker_deviation_t marker(struct image_t *input, uint8_t M)
52 {
53  struct marker_deviation_t marker_deviation;
54 
55  marker_deviation.x = 0;
56  marker_deviation.y = 0;
57  marker_deviation.inlier = 0;
58 
59  uint8_t *source = (uint8_t *) input->buf;
60  uint16_t x, y, i, j, k;
61 
62  if (M < 1) { M = 1; }
63  source = (uint8_t *) input->buf;
64 
65  int maxx = 160;
66  int maxy = 120;
67  int maxv = 0;
68 
69  for (j = M; j < (input->h - M); j++) {
70  for (i = M; i < (input->w - M); i++) {
71  int bad, good;
72  good = bad = 0;
73  for (k = 1; k < M; k+=2) {
74  // Pattern must be symmetric
75  bad += AbsDiff(Img(i - k, j) , Img(i + k, j));
76  bad += AbsDiff(Img(i, j - k) , Img(i, j + k));
77  bad += AbsDiff(Img(i - k, j - k) , Img(i + k, j + k));
78  bad += AbsDiff(Img(i + k, j - k) , Img(i - k, j + k));
79 
80  // Pattern: Must have perpendicular contrast
81  good += AbsDiff(Img(i - k, j) + Img(i + k, j), Img(i, j - k) + Img(i, j + k));
82  good += AbsDiff(Img(i - k, j - k) + Img(i + k, j + k), Img(i + k, j - k) + Img(i - k, j + k));
83  }
84 
85  for (k = 4; k < M; k += 2) {
86  // Pattern must be symmetric
87  bad += AbsDiff(Img(i - k, j - k / 2) , Img(i + k, j + k / 2));
88  bad += AbsDiff(Img(i + k / 2, j - k) , Img(i - k / 2, j + k));
89  bad += AbsDiff(Img(i - k / 2, j - k) , Img(i + k / 2, j + k));
90  bad += AbsDiff(Img(i + k, j - k / 2) , Img(i - k, j + k / 2));
91 
92  // Pattern: Must have perpendicular contrast
93  good += AbsDiff(Img(i - k, j - k / 2) + Img(i + k, j + k / 2), Img(i + k / 2, j - k) + Img(i - k / 2, j + k));
94  good += AbsDiff(Img(i - k / 2, j - k) + Img(i + k / 2, j + k), Img(i + k, j - k / 2) + Img(i - k, j + k / 2));
95  }
96 
97  int v = good - bad;
98  if (v < 0) {
99  v = 0;
100  }
101 
102  if (v > maxv) {
103  maxv = v;
104  maxx = i;
105  maxy = j;
106  }
107 
108 // if (v > 0) {
109 // Out(i, j) = 0xff;
110 // }
111  }
112  }
113 
114  // Display the marker location and center-lines.
115  int px = maxx & 0xFFFe;
116  int py = maxy & 0xFFFe;
117 
118  for (y = 0; y < input->h-1; y++) {
119  Out(px, y) = 65;
120  Out(px+1, y) = 255;
121  }
122  for (x = 0; x < input->w-1; x+=2) {
123  Out(x, py) = 65;
124  Out(x+1, py) = 255;
125  }
126 
127  marker_deviation.x = maxx;
128  marker_deviation.y = maxy;
129  marker_deviation.inlier = 0;
130 
131  //printf("The number of inliers = %i\n", counter3);
132  return marker_deviation;
133 }
134 
#define B
if(GpsFixValid() &&e_identification_started)
#define A
Definition: image.h:44
#define Out(X, Y)
Definition: imavmarker.c:40
#define Img(X, Y)
Find the marker location.
Definition: imavmarker.c:39
int AbsDiff(int A, int B)
Definition: imavmarker.c:43
struct marker_deviation_t marker(struct image_t *input, uint8_t M)
Definition: imavmarker.c:51
Find a IMAV pattern.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98