mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[georeferencer] project camera frame vectors to the ground
This commit is contained in:
Christophe De Wagter
@@ -0,0 +1,23 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="cv_georeference" dir="computer_vision">
|
||||
<doc>
|
||||
<description>Geo-reference computer vision detections</description>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="georeference">
|
||||
<dl_setting var="focus_length" min="1" step="10" max="4000" shortname="Focus"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="cv_georeference.h"/>
|
||||
</header>
|
||||
<init fun="georeference_init()"/>
|
||||
<periodic fun="georeference_run()" freq="4" start="georeference_init()" autorun="TRUE" />
|
||||
<makefile>
|
||||
<file name="cv_georeference.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,184 @@
|
||||
/*
|
||||
* Copyright (C) C. De Wagter
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/computer_vision/cv_georeference.c"
|
||||
* @author C. De Wagter
|
||||
* Geo-reference computer vision detections
|
||||
*/
|
||||
|
||||
#include "modules/computer_vision/cv_georeference.h"
|
||||
|
||||
#include "math/pprz_trig_int.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
#include "state.h"
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
struct camera_frame_t {
|
||||
int32_t w; ///< Frame width [px]
|
||||
int32_t h; ///< Frame height [px]
|
||||
int32_t f; ///< Camera Focal length in [px]
|
||||
int32_t px; ///< Target pixel coordinate (left = 0)
|
||||
int32_t py; ///< Target pixel coordinate (top = 0)
|
||||
};
|
||||
|
||||
struct georeference_filter_t {
|
||||
struct Int32Vect3 x; ///< Target
|
||||
struct Int32Vect3 v; ///< Target Velocity
|
||||
int32_t P; ///< Covariance/Average-count
|
||||
};
|
||||
|
||||
struct georeference_t {
|
||||
struct Int32Vect3 target_i; ///< Target in pixels, with z being the focal length in pixels, x=up,y=right,out
|
||||
struct Int32Vect3 target_l; ///< Target in meters, relative to the drone in LTP frame
|
||||
|
||||
struct Int32Vect3 x_t; ///< Target coordinates NED
|
||||
|
||||
struct georeference_filter_t filter; ///< Filter waypoint location
|
||||
};
|
||||
|
||||
struct georeference_t geo;
|
||||
|
||||
void georeference_project(struct camera_frame_t *tar, int wp)
|
||||
{
|
||||
// Target direction in camera frame: Zero is looking down in body frames
|
||||
// Pixel with x (width) value 0 projects to the left (body-Y-axis)
|
||||
// and y = 0 (height) to the top (body-X-axis)
|
||||
VECT3_ASSIGN(geo.target_i,
|
||||
((tar->h / 2) - tar->py),
|
||||
(tar->px - (tar->w / 2)),
|
||||
(tar->f)
|
||||
);
|
||||
INT32_VECT3_LSHIFT(geo.target_i, geo.target_i, 4)
|
||||
|
||||
// Camera <-> Body
|
||||
// Looking down in body frame
|
||||
// Bebop has 180deg Z rotation in camera (butt up yields normal webcam)
|
||||
struct Int32RMat body_to_cam_rmat;
|
||||
INT32_MAT33_ZERO(body_to_cam_rmat);
|
||||
MAT33_ELMT(body_to_cam_rmat, 0, 0) = -1 << INT32_TRIG_FRAC;
|
||||
MAT33_ELMT(body_to_cam_rmat, 1, 1) = -1 << INT32_TRIG_FRAC;
|
||||
MAT33_ELMT(body_to_cam_rmat, 2, 2) = 1 << INT32_TRIG_FRAC;
|
||||
|
||||
struct Int32Vect3 target_b;
|
||||
int32_rmat_transp_vmult(&target_b, &body_to_cam_rmat, &geo.target_i);
|
||||
|
||||
// Body <-> LTP
|
||||
struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
|
||||
int32_rmat_transp_vmult(&geo.target_l, ltp_to_body_rmat, &target_b);
|
||||
|
||||
// target_l is now a scale-less [pix<<POS_FRAC] vector in LTP from the drone to the target
|
||||
// Divide by z-component to normalize the projection vector
|
||||
int32_t zi = geo.target_l.z;
|
||||
if (zi <= 0)
|
||||
{
|
||||
// Pointing up or horizontal -> no ground projection
|
||||
return;
|
||||
}
|
||||
|
||||
// Multiply with height above ground
|
||||
struct NedCoor_i *pos = stateGetPositionNed_i();
|
||||
int32_t zb = pos->z;
|
||||
geo.target_l.x *= zb;
|
||||
geo.target_l.y *= zb;
|
||||
|
||||
// Divide by z-component
|
||||
geo.target_l.x /= zi;
|
||||
geo.target_l.y /= zi;
|
||||
geo.target_l.z = zb;
|
||||
|
||||
// NED
|
||||
geo.x_t.x = pos->x - geo.target_l.x;
|
||||
geo.x_t.y = pos->y - geo.target_l.y;
|
||||
geo.x_t.z = 0;
|
||||
|
||||
// ENU
|
||||
waypoint_set_xy_i(wp, geo.x_t.y, geo.x_t.x);
|
||||
waypoint_set_alt_i(wp, geo.x_t.z);
|
||||
}
|
||||
|
||||
void georeference_filter(bool_t kalman, int wp)
|
||||
{
|
||||
struct Int32Vect3 err;
|
||||
|
||||
if (kalman)
|
||||
{
|
||||
// Predict
|
||||
VECT3_ADD(geo.filter.x, geo.filter.v);
|
||||
|
||||
// Error = prediction - observation
|
||||
VECT3_COPY(err,geo.filter.x);
|
||||
VECT3_SUB(err, geo.x_t);
|
||||
}
|
||||
else // Average
|
||||
{
|
||||
VECT3_SMUL(geo.filter.x,geo.filter.x,geo.filter.P);
|
||||
VECT3_ADD(geo.filter.x, geo.x_t);
|
||||
geo.filter.P++;
|
||||
VECT3_SDIV(geo.filter.x,geo.filter.x,geo.filter.P);
|
||||
}
|
||||
|
||||
// ENU
|
||||
waypoint_set_xy_i(wp, geo.filter.x.y, geo.filter.x.x);
|
||||
waypoint_set_alt_i(wp, geo.filter.x.z);
|
||||
}
|
||||
|
||||
int32_t focus_length;
|
||||
|
||||
void georeference_run(void)
|
||||
{
|
||||
struct camera_frame_t target;
|
||||
target.w = 320;
|
||||
target.h = 240;
|
||||
target.f = focus_length;
|
||||
target.px = 0;
|
||||
target.py = 0;
|
||||
georeference_project(&target,WP_p1);
|
||||
target.px = 320;
|
||||
target.py = 0;
|
||||
georeference_project(&target,WP_p2);
|
||||
target.px = 320;
|
||||
target.py = 240;
|
||||
georeference_project(&target,WP_p3);
|
||||
target.px = 0;
|
||||
target.py = 240;
|
||||
georeference_project(&target,WP_p4);
|
||||
|
||||
target.px = 0;
|
||||
target.py = 120;
|
||||
georeference_project(&target,0);
|
||||
georeference_filter(FALSE, WP_CAM);
|
||||
}
|
||||
|
||||
void georeference_init(void)
|
||||
{
|
||||
INT32_VECT3_ZERO(geo.target_i);
|
||||
INT32_VECT3_ZERO(geo.target_l);
|
||||
|
||||
VECT3_ASSIGN(geo.x_t, 0, 0, 0);
|
||||
|
||||
INT32_VECT3_ZERO(geo.filter.v);
|
||||
INT32_VECT3_ZERO(geo.filter.x);
|
||||
geo.filter.P = 0;
|
||||
focus_length = 400;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* Copyright (C) C. De Wagter
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/computer_vision/cv_georeference.h"
|
||||
* @author C. De Wagter
|
||||
* Geo-reference computer vision detections
|
||||
*/
|
||||
|
||||
#ifndef CV_GEOREFERENCE_H
|
||||
#define CV_GEOREFERENCE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
extern int32_t focus_length;
|
||||
|
||||
extern void georeference_init(void);
|
||||
extern void georeference_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user