Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 18:51:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

unwrap_pano module for panoramic cameras (#2269)

Browse Source 
* Create unwrap_pano module

Create basic settings structure.

* Get basic image unwrapping working

* Add module settings

* Fix incorrect declaration of pano_unwrap_init()

* Add video_thread dependency

* Fix incorrect minus sign on real drone

* Set default forward direction for ARDrone2

* Set default vertical resolution to 0.18

Seems to be a better match with the gazebo model.

* Use LUT for unwrapping

LUT seems to work fine in simulation, except it crashes when
overwrite_video_thread is set to false...?

* FIX Add workaround for overwrite disable crash

* Add calibration pattern

* Expand module documentation

BUG: Segfault on startup on real drone, outside of camera_cb.

* Add example images to documentation

* Remove unused code and printf's

* Add max FPS define

* WIP add to ardrone2 example

* Fix #2187

* Clean up merge and temporary changes
This commit is contained in:
Tom van Dijk
2018-05-30 23:05:43 +02:00
committed by OpenUAS
parent c0741e01ab
commit 829f8d7796
6 changed files with 465 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
conf/modules/pano_unwrap.xml Normal file
View File

@@ -0,0 +1,82 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="pano_unwrap">
<doc>
<description>
Unwrap images taken through a panoramic lens
@image html images/airborne/modules/pano_unwrap/pano_unwrap.jpg "Left: AR.Drone 2.0 with Kogeto Dot 360 panoramic lens on bottom camera. Middle: raw image and region of interest (blue). Right: unwrapped image."
This module unwraps images taken through a panoramic lens. The unwrapped
image is returned to the video thread or available as
`pano_unwrapped_image`.
The user should specify the region of interest (the part of the raw image
that should be unwrapped) by setting the following values:
- The center of the lens (`center_x` and `center_y`)
- The radii of the top and bottom of the ROI (`radius_bottom` and
`radius_top`)
- The body +x direction in the raw image (`forward_direction`)
- The body +y direction in the raw image (`flip_horizontal`)
The region of interest can be shown by setting `show_calibration` to TRUE
and `overwrite_video` to FALSE.
The size of the output image can be set with `width` and `height`. The
height of the image can also be set to zero, in which case it is determined
automatically from the `vertical_resolution`.
Optionally, the image can be derotated to cancel the pitch and roll
movements of the drone. This requires a correct setting of the
`vertical_resolution`. Reasonable values can be found by looking at the
unwrapped and derotated image and adjusting the vertical resolution until
the environment no longer appears to move during pitch and roll motions.
The following assumptions are made:
- The vertical resolution is constant throughout the region of interest.
- Pitch and roll angles are small (for derotation).
@image html images/airborne/modules/pano_unwrap/unwrapped_derotated.png "Derotation (shown for large roll angle)."
</description>
<define name="PANO_UNWRAP_CAMERA" value="front_camera|bottom_camera (default)" description="Camera to use"/>
<define name="PANO_UNWRAP_CENTER_X" value="0.50" description="Center of lens in raw image [fraction of raw image width]"/>
<define name="PANO_UNWRAP_CENTER_Y" value="0.50" description="Center of lens in raw image [fraction of raw image height]"/>
<define name="PANO_UNWRAP_RADIUS_BOTTOM" value="0.20" description="Distance from the lens center to the bottom of the region of interest [fraction of raw image height]"/>
<define name="PANO_UNWRAP_RADIUS_TOP" value="0.30" description="Distance from lens center to the top of the region of interest [fraction of raw image height]"/>
<define name="PANO_UNWRAP_FORWARD_DIRECTION" value="270.0" description="Forward direction in raw image measured counterclockwise from right [deg]"/>
<define name="PANO_UNWRAP_FLIP_HORIZONTAL" value="TRUE|FALSE (default)" description="Flip output image horizontally"/>
<define name="PANO_UNWRAP_VERTICAL_RESOLUTION" value="0.18" description="Vertical resolution of region of interest [fraction of raw image height/rad]"/>
<define name="PANO_UNWRAP_DEROTATE_ATTITUDE" value="TRUE|FALSE (default)" description="Derotate the image, i.e. keep the panorama aligned with the horizon"/>
<define name="PANO_UNWRAP_WIDTH" value="640" description="Width of the unwrapped image [px]"/>
<define name="PANO_UNWRAP_HEIGHT" value="0" description="Height of the unwrapped image [px]. Set to 0 to determine automatically."/>
<define name="PANO_UNWRAP_OVERWRITE_VIDEO_THREAD" value="TRUE (default)|FALSE" description="Write unwrapped image to the video thread"/>
<define name="PANO_UNWRAP_FPS" value="0" description="Maximum FPS (0: unlimited)"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="pano_unwrap">
<dl_setting shortname="center_x" var="pano_unwrap.center.x" min="0.0" step="0.01" max="1.0"/>
<dl_setting shortname="center_y" var="pano_unwrap.center.y" min="0.0" step="0.01" max="1.0"/>
<dl_setting shortname="radius_bottom" var="pano_unwrap.radius_bottom" min="0.0" step="0.01" max="1.0"/>
<dl_setting shortname="radius_top" var="pano_unwrap.radius_top" min="0.0" step="0.01" max="1.0"/>
<dl_setting shortname="forward_direction" var="pano_unwrap.forward_direction" min="0" step="1" max="360"/>
<dl_setting shortname="flip_hor" var="pano_unwrap.flip_horizontal" type="uint8" values="FALSE|TRUE" min="0" step="1" max="1"/>
<dl_setting shortname="v_resolution" var="pano_unwrap.vertical_resolution" min="-0.2" step="0.01" max="0.2"/>
<dl_setting shortname="derotate" var="pano_unwrap.derotate_attitude" type="uint8" values="FALSE|TRUE" min="0" step="1" max="1"/>
<dl_setting shortname="width" var="pano_unwrap.width" min="1" step="1" max="1920"/>
<dl_setting shortname="height" var="pano_unwrap.height" min="0" step="1" max="1080"/>
<dl_setting shortname="overwrite_video" var="pano_unwrap.overwrite_video_thread" type="uint8" values="FALSE|TRUE" min="0" step="1" max="1"/>
<dl_setting shortname="show_calibration" var="pano_unwrap.show_calibration" type="uint8" values="FALSE|TRUE" min="0" step="1" max="1"/>
</dl_settings>
</dl_settings>
</settings>
<depends>video_thread</depends>
<header>
<file name="pano_unwrap.h"/>
</header>
<init fun="pano_unwrap_init()"/>
<makefile>
<file name="pano_unwrap.c"/>
<define name="CV_ALLOW_VIDEO_TO_CHANGE_SIZE" value="1"/>
</makefile>
</module>

BIN
doc/images/airborne/modules/pano_unwrap/pano_unwrap.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 125 KiB

BIN
doc/images/airborne/modules/pano_unwrap/unwrapped_derotated.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 258 KiB

7
sw/airborne/modules/computer_vision/cv.c
View File

@@ -113,6 +113,13 @@ int8_t cv_async_function(struct cv_async *async, struct image_t *img)
if (async->img_copy.buf_size == 0) {
image_create(&async->img_copy, img->w, img->h, img->type);
}
#if CV_ALLOW_VIDEO_TO_CHANGE_SIZE
// Note: must be enabled explicitly as not all modules may support this. (See issue #2187)
if (img->buf_size > async->img_copy.buf_size) {
image_free(&async->img_copy);
image_create(&async->img_copy, img->w, img->h, img->type);
}
#endif
// Copy image
// TODO:this takes time causing some thread lag, should be replaced with gpu operation

319
sw/airborne/modules/pano_unwrap/pano_unwrap.c Normal file
View File

@@ -0,0 +1,319 @@
/*
* Copyright (C) Tom van Dijk
*
* 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/pano_unwrap/pano_unwrap.c"
* @author Tom van Dijk
* Unwrap images taken through a panoramic lens.
*/
#include "modules/pano_unwrap/pano_unwrap.h"
#include "state.h"
#include "modules/computer_vision/cv.h"
#include <stdio.h>
#ifndef PANO_UNWRAP_CAMERA
#define PANO_UNWRAP_CAMERA bottom_camera
#endif
#ifndef PANO_UNWRAP_CENTER_X
#define PANO_UNWRAP_CENTER_X 0.50
#endif
#ifndef PANO_UNWRAP_CENTER_Y
#define PANO_UNWRAP_CENTER_Y 0.50
#endif
#ifndef PANO_UNWRAP_RADIUS_BOTTOM
#define PANO_UNWRAP_RADIUS_BOTTOM 0.20
#endif
#ifndef PANO_UNWRAP_RADIUS_TOP
#define PANO_UNWRAP_RADIUS_TOP 0.30
#endif
#ifndef PANO_UNWRAP_FORWARD_DIRECTION
#define PANO_UNWRAP_FORWARD_DIRECTION 270.0
#endif
#ifndef PANO_UNWRAP_FLIP_HORIZONTAL
#define PANO_UNWRAP_FLIP_HORIZONTAL FALSE
#endif
#ifndef PANO_UNWRAP_VERTICAL_RESOLUTION
#define PANO_UNWRAP_VERTICAL_RESOLUTION 0.18
#endif
#ifndef PANO_UNWRAP_DEROTATE_ATTITUDE
#define PANO_UNWRAP_DEROTATE_ATTITUDE FALSE
#endif
#ifndef PANO_UNWRAP_WIDTH
#define PANO_UNWRAP_WIDTH 640
#endif
#ifndef PANO_UNWRAP_HEIGHT
#define PANO_UNWRAP_HEIGHT 0
#endif
#ifndef PANO_UNWRAP_OVERWRITE_VIDEO_THREAD
#define PANO_UNWRAP_OVERWRITE_VIDEO_THREAD TRUE
#endif
#ifndef PANO_UNWRAP_FPS
#define PANO_UNWRAP_FPS 0
#endif
struct pano_unwrap_t pano_unwrap = {
.center = {
.x = PANO_UNWRAP_CENTER_X,
.y = PANO_UNWRAP_CENTER_Y,
},
.radius_bottom = PANO_UNWRAP_RADIUS_BOTTOM,
.radius_top = PANO_UNWRAP_RADIUS_TOP,
.forward_direction = PANO_UNWRAP_FORWARD_DIRECTION,
.flip_horizontal = PANO_UNWRAP_FLIP_HORIZONTAL,
.vertical_resolution = PANO_UNWRAP_VERTICAL_RESOLUTION,
.derotate_attitude = PANO_UNWRAP_DEROTATE_ATTITUDE,
.width = PANO_UNWRAP_WIDTH,
.height = PANO_UNWRAP_HEIGHT,
.overwrite_video_thread = PANO_UNWRAP_OVERWRITE_VIDEO_THREAD,
.show_calibration = FALSE,
};
struct image_t pano_unwrapped_image;
#define PIXEL_U(img,x,y) ( ((uint8_t*)((img)->buf))[4*(int)((x)/2) + 2*(y)*(img)->w] )
#define PIXEL_V(img,x,y) ( ((uint8_t*)((img)->buf))[4*(int)((x)/2) + 2*(y)*(img)->w + 2] )
#define PIXEL_Y(img,x,y) ( ((uint8_t*)((img)->buf))[2*(x) + 1 + 2*(y)*(img)->w] )
#define RED_Y 76
#define RED_U 84
#define RED_V 255
#define GREEN_Y 149
#define GREEN_U 43
#define GREEN_V 21
#define BLUE_Y 29
#define BLUE_U 255
#define BLUE_V 107
struct LUT_t
{
// Unwarping LUT
// For each pixel (u,v) in the unwrapped image, contains pixel coordinates of
// the same pixel in the raw image (x(u,v), y(u,v)).
uint16_t *x;
uint16_t *y;
// Derotate LUT
// For each bearing/column (u), contains the direction in which sampling should be
// shifted based on euler angles phi and theta dxy(phi|u) dxy(theta|u).
struct FloatVect2 *dphi;
struct FloatVect2 *dtheta;
// Settings for which the LUT was generated
struct pano_unwrap_t settings;
};
static struct LUT_t LUT; // Note: initialized NULL
static void set_output_image_size(void)
{
// Find vertical size of output image
if (pano_unwrap.height == 0) {
pano_unwrap.height = (uint16_t) (fabsf((pano_unwrap.width / (2 * M_PI))
* (pano_unwrap.radius_bottom - pano_unwrap.radius_top)
/ pano_unwrap.vertical_resolution) + 0.5);
printf("[pano_unwrap] Automatic output image height: %d\n",
pano_unwrap.height);
}
// Replace output image if required
if (pano_unwrapped_image.w != pano_unwrap.width ||
pano_unwrapped_image.h != pano_unwrap.height) {
printf("[pano_unwrap] Creating output image with size %d x %d\n",
pano_unwrap.width, pano_unwrap.height);
image_free(&pano_unwrapped_image);
image_create(&pano_unwrapped_image, pano_unwrap.width, pano_unwrap.height,
IMAGE_YUV422);
}
}
static void update_LUT(const struct image_t *img)
{
if (LUT.settings.center.x == pano_unwrap.center.x &&
LUT.settings.center.y == pano_unwrap.center.y &&
LUT.settings.radius_bottom == pano_unwrap.radius_bottom &&
LUT.settings.radius_top == pano_unwrap.radius_top &&
LUT.settings.forward_direction == pano_unwrap.forward_direction &&
LUT.settings.flip_horizontal == pano_unwrap.flip_horizontal &&
LUT.settings.width == pano_unwrap.width &&
LUT.settings.height == pano_unwrap.height) {
// LUT is still valid, do nothing
return;
}
printf("[pano_unwrap] Regenerating LUT... ");
// Remove old data
if (LUT.x) {
free(LUT.x);
LUT.x = NULL;
}
if (LUT.y) {
free(LUT.y);
LUT.y = NULL;
}
if (LUT.dphi) {
free(LUT.dphi);
LUT.dphi = NULL;
}
if (LUT.dtheta) {
free(LUT.dtheta);
LUT.dtheta = NULL;
}
// Generate unwarping LUT
LUT.x = malloc(sizeof(*LUT.x) * pano_unwrap.width * pano_unwrap.height);
LUT.y = malloc(sizeof(*LUT.y) * pano_unwrap.width * pano_unwrap.height);
if (!LUT.x || !LUT.y) {
printf("[pano_unwrap] ERROR could not allocate x or y lookup table!\n");
} else {
for (uint16_t u = 0; u < pano_unwrap.width; u++) {
float bearing = -M_PI + (float) u / pano_unwrapped_image.w * 2 * M_PI;
float angle = (pano_unwrap.forward_direction / 180.0 * M_PI)
+ bearing * (pano_unwrap.flip_horizontal ? 1.f : -1.f);
float c = cosf(angle);
float s = sinf(angle);
for (uint16_t v = 0; v < pano_unwrap.height; v++) {
float radius = pano_unwrap.radius_top
+ (pano_unwrap.radius_bottom - pano_unwrap.radius_top)
* ((float) v / (pano_unwrap.height - 1));
LUT.x[v + u * pano_unwrap.height] = (uint16_t) (img->w
* pano_unwrap.center.x + c * radius * img->h + 0.5);
LUT.y[v + u * pano_unwrap.height] = (uint16_t) (img->h
* pano_unwrap.center.y - s * radius * img->h + 0.5);
}
}
}
// Generate derotation LUT
LUT.dphi = malloc(sizeof(*LUT.dphi) * pano_unwrap.width);
LUT.dtheta = malloc(sizeof(*LUT.dtheta) * pano_unwrap.width);
if (!LUT.dphi || !LUT.dtheta) {
printf(
"[pano_unwrap] ERROR could not allocate dphi or dtheta lookup table!\n");
} else {
for (uint16_t u = 0; u < pano_unwrap.width; u++) {
float bearing = -M_PI + (float) u / pano_unwrapped_image.w * 2 * M_PI;
float angle = (pano_unwrap.forward_direction / 180.0 * M_PI)
+ bearing * (pano_unwrap.flip_horizontal ? 1.f : -1.f);
LUT.dphi[u].x = sinf(bearing) * cosf(angle);
LUT.dphi[u].y = sinf(bearing) * -sinf(angle);
LUT.dtheta[u].x = cosf(bearing) * cosf(angle);
LUT.dtheta[u].y = cosf(bearing) * -sinf(angle);
}
}
// Keep track of settings for which this LUT was generated
LUT.settings = pano_unwrap;
printf("ok\n");
}
static void unwrap_LUT(struct image_t *img_raw, struct image_t *img)
{
for (uint16_t u = 0; u < pano_unwrap.width; u++) {
// Derotation offset for this bearing
int16_t dx = 0;
int16_t dy = 0;
if (pano_unwrap.derotate_attitude) {
// Look up correction
const struct FloatRMat *R = stateGetNedToBodyRMat_f();
dx = (int16_t) (img_raw->h * pano_unwrap.vertical_resolution
* (LUT.dphi[u].x * MAT33_ELMT(*R, 1, 2)
+ LUT.dtheta[u].x * MAT33_ELMT(*R, 0, 2)));
dy = (int16_t) (img_raw->h * pano_unwrap.vertical_resolution
* (LUT.dphi[u].y * MAT33_ELMT(*R, 1, 2)
+ LUT.dtheta[u].y * MAT33_ELMT(*R, 0, 2)));
}
// Fill this column of unwrapped image
for (uint16_t v = 0; v < pano_unwrap.height; v++) {
// Look up sampling point
int16_t x = (int16_t) LUT.x[v + u * pano_unwrap.height] + dx;
int16_t y = (int16_t) LUT.y[v + u * pano_unwrap.height] + dy;
// Check bounds
if (x < 0) {
x = 0;
} else if (x > img_raw->w - 1) {
x = img_raw->w - 1;
}
if (y < 0) {
y = 0;
} else if (y > img_raw->h - 1) {
y = img_raw->h - 1;
}
// Draw calibration pattern
if (pano_unwrap.show_calibration) {
if (v == 0 || v == pano_unwrap.height - 1) {
PIXEL_U(img_raw,x,y) = BLUE_U;
PIXEL_V(img_raw,x,y) = BLUE_V;
}
if ((u == pano_unwrap.width / 2)
|| (u == pano_unwrap.width / 2 + 1)) {
PIXEL_Y(img_raw,x,y) = RED_Y;
PIXEL_U(img_raw,x,y) = RED_U;
PIXEL_V(img_raw,x,y) = RED_V;
}
if ((u == 3 * pano_unwrap.width / 4)
|| (u == 3 * pano_unwrap.width / 4 + 1)) {
PIXEL_Y(img_raw,x,y) = GREEN_Y;
PIXEL_U(img_raw,x,y) = GREEN_U;
PIXEL_V(img_raw,x,y) = GREEN_V;
}
}
// Copy pixel values
PIXEL_Y(img,u,v) = PIXEL_Y(img_raw, x, y);
PIXEL_U(img,u,v) = PIXEL_U(img_raw, x, y);
PIXEL_V(img,u,v) = PIXEL_V(img_raw, x, y);
}
}
// Draw lens center
if (pano_unwrap.show_calibration) {
uint16_t x = pano_unwrap.center.x * img_raw->w;
uint16_t y = pano_unwrap.center.y * img_raw->h;
for (int i = -5; i <= 5; i++) {
for (int j = -5; j <= 5; j++) {
if (i == 0 || j == 0) {
PIXEL_Y(img_raw, x+i, y+j) = BLUE_Y;
PIXEL_U(img_raw, x+i, y+j) = BLUE_U;
PIXEL_V(img_raw, x+i, y+j) = BLUE_V;
}
}
}
}
}
static struct image_t *camera_cb(struct image_t *img)
{
set_output_image_size();
update_LUT(img);
unwrap_LUT(img, &pano_unwrapped_image);
pano_unwrapped_image.ts = img->ts;
pano_unwrapped_image.pprz_ts = img->pprz_ts;
return pano_unwrap.overwrite_video_thread ? &pano_unwrapped_image : NULL;
}
void pano_unwrap_init()
{
image_create(&pano_unwrapped_image, 0, 0, IMAGE_YUV422);
set_output_image_size();
cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS);
}

57
sw/airborne/modules/pano_unwrap/pano_unwrap.h Normal file
View File

@@ -0,0 +1,57 @@
/*
* Copyright (C) Tom van Dijk
*
* 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/pano_unwrap/pano_unwrap.h"
* @author Tom van Dijk
* Unwrap images taken through a panoramic lens.
*/
#ifndef PANO_UNWRAP_H
#define PANO_UNWRAP_H
#include "modules/computer_vision/lib/vision/image.h"
struct pano_unwrap_t
{
struct FloatVect2 center; ///< Center point of panoramic lens [fraction of image width, height]
float radius_bottom; ///< Distance from center point to bottom of region of interest [fraction of image height]
float radius_top; ///< Distance from center point to top of region of interest [fraction of image height]
float forward_direction; ///< Angle [deg] in raw image that corresponds to the forward direction, where 0 points right and the value increases counterclockwise.
bool flip_horizontal; ///< Set to true to horizontally flip the unwrapped image.
float vertical_resolution; ///< Vertical resolution of raw image in the region of interest, used for attitude derotation [fraction of image height/rad] (Note: negative values are allowed)
bool derotate_attitude; ///< Set to true if roll/pitch movement should be corrected.
uint16_t width; ///< Width of unwrapped image
uint16_t height; ///< Height of unwrapped image. Set to 0 (default) to determine automatically from unwrapped_width, radius_bottom, _top and vertical_resolution.
bool overwrite_video_thread; ///< Set to true if the unwrapped image should be returned to the video thread.
bool show_calibration; ///< Draw calibration pattern on raw image.
};
extern struct pano_unwrap_t pano_unwrap;
/// Unwrapped panoramic image
extern struct image_t pano_unwrapped_image;
extern void pano_unwrap_init(void);
#endif