Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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;
164  target.px = 320;
165  target.py = 0;
167  target.px = 320;
168  target.py = 240;
170  target.px = 0;
171  target.py = 240;
173 
174  target.px = 0;
175  target.py = 120;
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 
georeference_t::filter
struct georeference_filter_t filter
Filter waypoint location.
Definition: cv_georeference.c:48
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
camera_frame_t
Definition: cv_georeference.h:37
VECT3_SMUL
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
INT32_MAT33_ZERO
#define INT32_MAT33_ZERO(_m)
Definition: pprz_algebra_int.h:315
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
cv_georeference.h
focus_length
int32_t focus_length
Definition: cv_georeference.c:153
waypoint_set_alt_i
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
Definition: waypoints.c:159
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
georeference_filter
void georeference_filter(bool kalman, int wp, int length)
Definition: cv_georeference.c:119
georeference_init
void georeference_init(void)
Definition: cv_georeference.c:180
VECT3_SDIV
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
VECT3_SUB
#define VECT3_SUB(_a, _b)
Definition: pprz_algebra.h:154
camera_frame_t::px
int32_t px
Target pixel coordinate (left = 0)
Definition: cv_georeference.h:41
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
MAT33_ELMT
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
geo
struct georeference_t geo
Definition: cv_georeference.c:51
waypoint_set_xy_i
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
pprz_algebra_int.h
Paparazzi fixed point algebra.
georeference_filter_t
Definition: cv_georeference.c:36
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
camera_frame_t::py
int32_t py
Target pixel coordinate (top = 0)
Definition: cv_georeference.h:42
georeference_t::target_i
struct Int32Vect3 target_i
Target in pixels, with z being the focal length in pixels, x=up,y=right,out.
Definition: cv_georeference.c:43
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:288
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
georeference_t
Definition: cv_georeference.c:42
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:117
georeference_t::target_l
struct Int32Vect3 target_l
Target in meters, relative to the drone in LTP frame.
Definition: cv_georeference.c:44
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
georeference_filter_t::v
struct Int32Vect3 v
Target Velocity.
Definition: cv_georeference.c:38
Int32Vect3
Definition: pprz_algebra_int.h:88
uint8_t
unsigned char uint8_t
Definition: types.h:14
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
INT32_VECT3_LSHIFT
#define INT32_VECT3_LSHIFT(_o, _i, _l)
Definition: pprz_algebra_int.h:304
pprz_trig_int.h
Paparazzi fixed point trig functions.
camera_frame_t::w
int32_t w
Frame width [px].
Definition: cv_georeference.h:38
georeference_project
void georeference_project(struct camera_frame_t *tar, int wp)
Definition: cv_georeference.c:53
georeference_filter_t::P
int32_t P
Covariance/Average-count.
Definition: cv_georeference.c:39
georeference_run
void georeference_run(void)
Definition: cv_georeference.c:155
georeference_t::x_t
struct Int32Vect3 x_t
Target coordinates NED.
Definition: cv_georeference.c:46
int32_t
signed long int32_t
Definition: types.h:19
camera_frame_t::f
int32_t f
Camera Focal length in [px].
Definition: cv_georeference.h:40
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
georeference_filter_t::x
struct Int32Vect3 x
Target.
Definition: cv_georeference.c:37
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
pprz_algebra.h
Paparazzi generic algebra macros.
camera_frame_t::h
int32_t h
Frame height [px].
Definition: cv_georeference.h:39
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
FALSE
#define FALSE
Definition: std.h:5
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140