Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
42inline int AbsDiff(int A, int B);
43inline int AbsDiff(int A, int B)
44{
45 if (A < B) {
46 return B - A;
47 }
48 return A - B;
49}
50
52{
54
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
129 marker_deviation.inlier = 0;
130
131 //printf("The number of inliers = %i\n", counter3);
132 return marker_deviation;
133}
134
#define B
#define A
#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.
uint16_t foo
Definition main_demo5.c:58
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.