Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cv_georeference.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) C. De Wagter
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  */
27 
28 #include "math/pprz_trig_int.h"
29 #include "math/pprz_algebra.h"
30 #include "math/pprz_algebra_int.h"
31 
32 #include "state.h"
33 #include "generated/flight_plan.h"
35 
37  struct Int32Vect3 x;
38  struct Int32Vect3 v;
40 };
41 
45 
46  struct Int32Vect3 x_t;
47 
49 };
50 
52 
53 void georeference_project(struct camera_frame_t *tar, int wp)
54 {
55  // Target direction in camera frame: Zero is looking down in body frames
56  // Pixel with x (width) value 0 projects to the left (body-Y-axis)
57  // and y = 0 (height) to the top (body-X-axis)
59  ((tar->h / 2) - tar->py),
60  (tar->px - (tar->w / 2)),
61  (tar->f)
62  );
64 
65  // Camera <-> Body
66  // Looking down in body frame
67  // Bebop has 180deg Z rotation in camera (butt up yields normal webcam)
68  struct Int32RMat body_to_cam_rmat;
69  INT32_MAT33_ZERO(body_to_cam_rmat);
70  MAT33_ELMT(body_to_cam_rmat, 0, 0) = -1 << INT32_TRIG_FRAC;
71  MAT33_ELMT(body_to_cam_rmat, 1, 1) = -1 << INT32_TRIG_FRAC;
72  MAT33_ELMT(body_to_cam_rmat, 2, 2) = 1 << INT32_TRIG_FRAC;
73 
74  struct Int32Vect3 target_b;
75  int32_rmat_transp_vmult(&target_b, &body_to_cam_rmat, &geo.target_i);
76 
77  // Body <-> LTP
78  struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
79  int32_rmat_transp_vmult(&geo.target_l, ltp_to_body_rmat, &target_b);
80 
81  // target_l is now a scale-less [pix<<POS_FRAC] vector in LTP from the drone to the target
82  // Divide by z-component to normalize the projection vector
83  int32_t zi = geo.target_l.z;
84  if (zi <= 0)
85  {
86  // Pointing up or horizontal -> no ground projection
87  return;
88  }
89 
90  // Multiply with height above ground
91  struct NedCoor_i *pos = stateGetPositionNed_i();
92  int32_t zb = pos->z;
93  geo.target_l.x *= zb;
94  geo.target_l.y *= zb;
95 
96  // Divide by z-component
97  geo.target_l.x /= zi;
98  geo.target_l.y /= zi;
99  geo.target_l.z = zb;
100 
101  // NED
102  geo.x_t.x = pos->x - geo.target_l.x;
103  geo.x_t.y = pos->y - geo.target_l.y;
104  geo.x_t.z = 0;
105 
106  // ENU
107  if (wp > 0) {
110 
111  int32_t h = -geo.x_t.z;
112  uint8_t wp_id = wp;
113  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(geo.x_t.y),
114  &(geo.x_t.x), &(h));
115 
116  }
117 }
118 
119 void georeference_filter(bool kalman, int wp, int length)
120 {
121  struct Int32Vect3 err;
122 
123  if (kalman)
124  {
125  // Predict
127 
128  // Error = prediction - observation
129  VECT3_COPY(err,geo.filter.x);
130  VECT3_SUB(err, geo.x_t);
131  }
132  else // Average
133  {
136  geo.filter.P++;
138  if (geo.filter.P > length) {
139  geo.filter.P = length;
140  }
141  }
142 
143  // ENU
145  //waypoint_set_alt_i(wp, geo.filter.x.z);
146 
147  int32_t h = 0;
148  uint8_t wp_id = wp;
149  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(geo.filter.x.y),
150  &(geo.filter.x.x), &(h));
151 }
152 
154 
156 {
157  struct camera_frame_t target;
158  target.w = 320;
159  target.h = 240;
160  target.f = focus_length;
161  target.px = 0;
162  target.py = 0;
163  georeference_project(&target,WP_p1);
164  target.px = 320;
165  target.py = 0;
166  georeference_project(&target,WP_p2);
167  target.px = 320;
168  target.py = 240;
169  georeference_project(&target,WP_p3);
170  target.px = 0;
171  target.py = 240;
172  georeference_project(&target,WP_p4);
173 
174  target.px = 0;
175  target.py = 120;
176  georeference_project(&target,0);
177  georeference_filter(FALSE, WP_CAM,50);
178 }
179 
181 {
184 
185  VECT3_ASSIGN(geo.x_t, 0, 0, 0);
186 
189  geo.filter.P = 0;
190  focus_length = 400;
191 }
192 
193 
#define VECT3_SUB(_a, _b)
Definition: pprz_algebra.h:154
int32_t px
Target pixel coordinate (left = 0)
#define INT32_VECT3_LSHIFT(_o, _i, _l)
struct georeference_filter_t filter
Filter waypoint location.
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define INT32_MAT33_ZERO(_m)
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
Definition: waypoints.c:159
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define INT32_VECT3_ZERO(_v)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
struct Int32Vect3 target_l
Target in meters, relative to the drone in LTP frame.
Paparazzi fixed point trig functions.
int32_t z
Down.
int32_t focus_length
struct georeference_t geo
void georeference_init(void)
#define FALSE
Definition: std.h:5
int32_t y
East.
void georeference_run(void)
void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Set only local XY coordinates of waypoint without update altitude.
Definition: waypoints.c:138
void georeference_project(struct camera_frame_t *tar, int wp)
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
void georeference_filter(bool kalman, int wp, int length)
struct Int32Vect3 x_t
Target coordinates NED.
int32_t py
Target pixel coordinate (top = 0)
Paparazzi generic algebra macros.
struct Int32Vect3 target_i
Target in pixels, with z being the focal length in pixels, x=up,y=right,out.
int32_t x
North.
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
struct Int32Vect3 x
Target.
signed long int32_t
Definition: types.h:19
int32_t P
Covariance/Average-count.
#define INT32_TRIG_FRAC
struct Int32Vect3 v
Target Velocity.
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
int32_t h
Frame height [px].
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t f
Camera Focal length in [px].
vector in North East Down coordinates
int32_t w
Frame width [px].
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
rotation matrix
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
Paparazzi fixed point algebra.