Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[vision] Move folders and add general image struct

Browse Source
This commit is contained in:
Freek van Tienen
2015-03-12 10:53:43 +01:00
parent 0e0d1be8d0
commit 9be322692f
23 changed files with 458 additions and 470 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/cv_opticflow.xml
+6 -4
View File
@@ -45,8 +45,9 @@
<makefile target="ap">
<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<!--file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/-->
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<!-- The optical flow module (calculator+stabilization) -->
@@ -55,13 +56,14 @@
<file name="stabilization_opticflow.c" dir="modules/computer_vision/opticflow"/>
<!-- Main vision calculations -->
<file name="fast_rosten.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="lucas_kanade.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
<file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
<!-- Random flags -->
<define name="__USE_GNU"/>
<flag name="LDFLAGS" value="lrt"/>
<flag name="LDFLAGS" value="static-libgcc"/>
<define name="OPTICFLOW_DEBUG"/>
</makefile>
<makefile target="nps">
conf/modules/video_rtp_stream.xml
+3 -2
View File
@@ -37,8 +37,9 @@
<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<!-- Define the network connection to send images over -->
sw/airborne/modules/computer_vision/cv/color.h
-88
View File
@@ -1,88 +0,0 @@
/*
* Copyright (C) 2012-2013
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdint.h>
#include "image.h"
inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output);
inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
source++;
for (int y = 0; y < output->h; y++) {
for (int x = 0; x < output->w; x++) {
// UYVY
*dest++ = 127; // U
*dest++ = *source; // Y
source += 2;
}
}
}
inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
uint8_t u_M, uint8_t v_m, uint8_t v_M);
inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
uint8_t u_M, uint8_t v_m, uint8_t v_M)
{
int cnt = 0;
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
for (int y = 0; y < output->h; y++) {
for (int x = 0; x < output->w; x += 2) {
// Color Check:
if (
// Light
(dest[1] >= y_m)
&& (dest[1] <= y_M)
&& (dest[0] >= u_m)
&& (dest[0] <= u_M)
&& (dest[2] >= v_m)
&& (dest[2] <= v_M)
) { // && (dest[2] > 128))
cnt ++;
// UYVY
dest[0] = 64; // U
dest[1] = source[1]; // Y
dest[2] = 255; // V
dest[3] = source[3]; // Y
} else {
// UYVY
char u = source[0] - 127;
u /= 4;
dest[0] = 127; // U
dest[1] = source[1]; // Y
u = source[2] - 127;
u /= 4;
dest[2] = 127; // V
dest[3] = source[3]; // Y
}
dest += 4;
source += 4;
}
}
return cnt;
}
sw/airborne/modules/computer_vision/cv/image.h
-35
View File
@@ -1,35 +0,0 @@
/*
* Copyright (C) 2012-2013
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef _MY_IMAGE_HEADER_
#define _MY_IMAGE_HEADER_
struct img_struct {
int seq;
double timestamp;
unsigned char *buf;
int w;
int h;
};
#endif
sw/airborne/modules/computer_vision/cv/resize.h
-61
View File
@@ -1,61 +0,0 @@
/*
* Copyright (C) 2012-2013
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdint.h>
#include "image.h"
/** Simplified high-speed low CPU downsample function without averaging
*
* downsample factor must be 1, 2, 4, 8 ... 2^X
* image of typ UYVY expected. Only one color UV per 2 pixels
*
* we keep the UV color of the first pixel pair
* and sample the intensity evenly 1-3-5-7-... or 1-5-9-...
*
* input: u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=1 u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=2 u1y1v1 (skip2) y3 (skip2) u5y5v5 (skip2 y7 (skip2) ...
* downsample=4 u1y1v1 (skip6) y5 (skip6) ...
*/
inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample);
inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
int pixelskip = (downsample - 1) * 2;
for (int y = 0; y < output->h; y++) {
for (int x = 0; x < output->w; x += 2) {
// YUYV
*dest++ = *source++; // U
*dest++ = *source++; // Y
*dest++ = *source++; // V
source += pixelskip;
*dest++ = *source++; // Y
source += pixelskip;
}
// read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first
source += (downsample-1) * input->w * 2;
}
}
sw/airborne/modules/computer_vision/cv/encoding/jpeg.c → sw/airborne/modules/computer_vision/lib/encoding/jpeg.c
+16 -10
View File
@@ -415,20 +415,24 @@ void MakeTables(int q)
}
}
uint8_t *jpeg_encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format,
uint32_t image_width, uint32_t image_height, bool_t add_dri_header)
/**
* Encode an YUV422 image
* @param[in] *in The input image
* @param[out] *out The output JPEG image
* @param[in] quality_factor Quality factor of the encoding (0-99)
* @param[in] add_dri_header Add the DRI header (needed for full JPEG)
*/
void jpeg_encode_image(struct image_t *in, struct image_t *out, uint32_t quality_factor, bool_t add_dri_header)
{
uint16_t i, j;
uint8_t *output_ptr = out->buf;
uint8_t *input_ptr = in->buf;
JPEG_ENCODER_STRUCTURE JpegStruct;
JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct;
/* Initialization of JPEG control structure */
jpeg_initialization(jpeg_encoder_structure, image_format, image_width, image_height);
jpeg_initialization(jpeg_encoder_structure, FOUR_TWO_TWO, in->w, in->h);
/* Quantization Table Initialization */
//jpeg_initialize_quantization_tables (quality_factor);
@@ -437,7 +441,7 @@ uint8_t *jpeg_encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t qua
/* Writing Marker Data */
if (add_dri_header) {
output_ptr = jpeg_write_markers(output_ptr, image_format, image_width, image_height);
output_ptr = jpeg_write_markers(output_ptr, FOUR_TWO_TWO, in->w, in->h);
}
for (i = 1; i <= jpeg_encoder_structure->vertical_mcus; i++) {
@@ -459,7 +463,7 @@ uint8_t *jpeg_encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t qua
read_format(jpeg_encoder_structure, input_ptr);
/* Encode the data in MCU */
output_ptr = jpeg_encodeMCU(jpeg_encoder_structure, image_format, output_ptr);
output_ptr = jpeg_encodeMCU(jpeg_encoder_structure, FOUR_TWO_TWO, output_ptr);
input_ptr += jpeg_encoder_structure->mcu_width_size;
}
@@ -469,7 +473,9 @@ uint8_t *jpeg_encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t qua
/* Close Routine */
output_ptr = jpeg_close_bitstream(output_ptr);
return output_ptr;
out->w = in->w;
out->h = in->h;
out->buf_size = output_ptr - (uint8_t*)out->buf;
}
static uint8_t *jpeg_encodeMCU(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint32_t image_format, uint8_t *output_ptr)
sw/airborne/modules/computer_vision/cv/encoding/jpeg.h → sw/airborne/modules/computer_vision/lib/encoding/jpeg.h
+2 -9
View File
@@ -22,6 +22,7 @@
#define _CV_ENCODING_JPEG_H
#include "std.h"
#include "lib/vision/image.h"
/* The different type of image encodings */
#define FOUR_ZERO_ZERO 0
@@ -31,15 +32,7 @@
#define RGB 4
/* JPEG encode an image */
unsigned char *jpeg_encode_image(
uint8_t *in,
uint8_t *out,
uint32_t q, // image quality 1-8
uint32_t fmt, // image format code
uint32_t width, // image width
uint32_t height, // image height
bool_t add_dri_header // data only or full jpeg file
);
void jpeg_encode_image(struct image_t *in, struct image_t *out, uint32_t quality_factor, bool_t add_dri_header);
/* Create an SVS header */
int jpeg_create_svs_header(unsigned char *buf, int32_t size, int w);
sw/airborne/modules/computer_vision/cv/encoding/rtp.c → sw/airborne/modules/computer_vision/lib/encoding/rtp.c
+10 -8
View File
@@ -87,12 +87,14 @@ void rtp_frame_test(struct udp_periph *udp)
/**
* Send an RTP frame
*/
void rtp_frame_send(struct udp_periph *udp, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code,
void rtp_frame_send(struct udp_periph *udp, struct image_t *img, uint8_t format_code,
uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
{
static uint32_t packetcounter = 0;
static uint32_t timecounter = 0;
uint32_t offset = 0;
uint32_t jpeg_size = img->buf_size;
uint8_t *jpeg_ptr = img->buf;
#define MAX_PACKET_SIZE 1400
@@ -104,20 +106,20 @@ void rtp_frame_send(struct udp_periph *udp, uint8_t *Jpeg, uint32_t JpegLen, int
}
// Split frame into packets
for (; JpegLen > 0;) {
for (; jpeg_size > 0;) {
uint32_t len = MAX_PACKET_SIZE;
uint8_t lastpacket = 0;
if (JpegLen <= len) {
if (jpeg_size <= len) {
lastpacket = 1;
len = JpegLen;
len = jpeg_size;
}
rtp_packet_send(udp, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code,
has_dri_header);
rtp_packet_send(udp, jpeg_ptr, len, packetcounter, timecounter, offset, lastpacket, img->w, img->h, format_code,
quality_code, has_dri_header);
JpegLen -= len;
Jpeg += len;
jpeg_size -= len;
jpeg_ptr += len;
offset += len;
packetcounter++;
}
sw/airborne/modules/computer_vision/cv/encoding/rtp.h → sw/airborne/modules/computer_vision/lib/encoding/rtp.h
+2 -2
View File
@@ -29,12 +29,12 @@
#define _CV_ENCODING_RTP_H
#include "std.h"
#include "lib/vision/image.h"
#include "mcu_periph/udp.h"
void rtp_frame_send(
struct udp_periph *udp, // socket
uint8_t *Jpeg, uint32_t JpegLen, // jpeg data
int w, int h, // width and height
struct image_t *img, // The image to send
uint8_t format_code, // 0=422, 1=421
uint8_t quality_code, // 0-99 of 128 for custom (include
uint8_t has_dri_header, // Does Jpeg data include Header Info?
sw/airborne/modules/computer_vision/lib/udp/socket.c
-104
View File
@@ -1,104 +0,0 @@
#include "socket.h"
#include <netdb.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <unistd.h>
#include <netdb.h>
#include <errno.h>
#include <netinet/in.h>
# define ADDR_SIZE_TYPE socklen_t
# define SOCKET_ERROR -1
# define IO_SOCKET ioctl
struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast)
{
struct UdpSocket *me = malloc(sizeof(struct UdpSocket));
int so_reuseaddr = 1;
me->socket_out = socket(AF_INET, SOCK_DGRAM, 0);
setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
/* only set broadcast option if explicitly enabled */
if (broadcast)
setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST,
&broadcast, sizeof(broadcast));
me->addr_out.sin_family = AF_INET;
me->addr_out.sin_port = htons(port_out);
me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out);
me->socket_in = socket(AF_INET, SOCK_DGRAM, 0);
setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
me->addr_in.sin_family = AF_INET;
me->addr_in.sin_port = htons(port_in);
me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in));
return me;
}
#include <stdio.h>
//#define UDP_MODE MSG_DONTWAIT
#define UDP_MODE 0
int udp_write(struct UdpSocket *me, unsigned char *buf, int len)
{
sendto(me->socket_out, buf, len, UDP_MODE,
(struct sockaddr *)&me->addr_out, sizeof(me->addr_out));
//printf("sendto ret=%d\n",ret);
return len;
}
unsigned long MIN(unsigned long a, unsigned long b);
unsigned long MIN(unsigned long a, unsigned long b)
{
if (a < b) { return a; }
return b;
}
int udp_read(struct UdpSocket *me, unsigned char *buf, int len)
{
unsigned long toread = 0;
int btr = 1; // set to >0 in order to start the reading loop
int newbytes = 0;
int status;
// if socket is connected
for (; btr > 0;) {
// Check Status
status = IO_SOCKET(me->socket_in, FIONREAD, &toread);
if (status == SOCKET_ERROR) {
printf("problem receiving from socket\n");
break;
}
//printf("UDP has %d bytes\n", toread);
if (toread <= 0) {
break;
}
// If status: ok and new data: read it
btr = MIN(toread, (unsigned long)len);
recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr *)&me->addr_in, (socklen_t *) sizeof(me->addr_in));
newbytes += btr;
}
return newbytes;
}
sw/airborne/modules/computer_vision/lib/udp/socket.h
-25
View File
@@ -1,25 +0,0 @@
#ifndef SOCKET_H
#define SOCKET_H
#include <sys/socket.h>
#include <arpa/inet.h>
#define FMS_UNICAST 0
#define FMS_BROADCAST 1
struct UdpSocket {
int socket_in;
int socket_out;
struct sockaddr_in addr_in;
struct sockaddr_in addr_out;
};
extern struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast);
extern int udp_write(struct UdpSocket *me, unsigned char *buf, int len);
extern int udp_read(struct UdpSocket *me, unsigned char *buf, int len);
#endif /* SOCKET_H */
sw/airborne/modules/computer_vision/lib/v4l/v4l2.c
+26 -10
View File
@@ -254,7 +254,6 @@ struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height
}
// Map the buffer
buffers[i].idx = i;
buffers[i].length = buf.length;
buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
if (MAP_FAILED == buffers[i].buf) {
@@ -282,8 +281,10 @@ struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height
* This functions blocks until image access is granted. This should not take that long, because
* it is only locked while enqueueing an image.
* Make sure you free the image after processing!
* @param[in] *dev The V4L2 video device we want to get an image from
* @param[out] *img The image that we got from the video device
*/
struct v4l2_img_buf *v4l2_image_get(struct v4l2_device *dev) {
void v4l2_image_get(struct v4l2_device *dev, struct image_t *img) {
uint16_t img_idx = V4L2_IMG_NONE;
// Continu to wait for an image
@@ -302,16 +303,24 @@ struct v4l2_img_buf *v4l2_image_get(struct v4l2_device *dev) {
}
}
// Rreturn the image
return &dev->buffers[img_idx];
// Set the image
img->type = IMAGE_YUV422;
img->w = dev->w;
img->h = dev->h;
img->buf_idx = img_idx;
img->buf = dev->buffers[img_idx].buf;
memcpy(&img->ts, &dev->buffers[img_idx].timestamp, sizeof(struct timeval));
}
/**
* Get the latest image and lock it (Thread safe, NON BLOCKING)
* This function returns NULL if it can't get access to the current image.
* Make sure you free the image after processing!
* @param[in] *dev The V4L2 video device we want to get an image from
* @param[out] *img The image that we got from the video device
* @return Whether we got an image or not
*/
struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev) {
bool_t v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img) {
uint16_t img_idx = V4L2_IMG_NONE;
// Try to get the current image
@@ -324,9 +333,16 @@ struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev) {
// Check if we really got an image
if (img_idx == V4L2_IMG_NONE) {
return NULL;
return FALSE;
} else {
return &dev->buffers[img_idx];
// Set the image
img->type = IMAGE_YUV422;
img->w = dev->w;
img->h = dev->h;
img->buf_idx = img_idx;
img->buf = dev->buffers[img_idx].buf;
memcpy(&img->ts, &dev->buffers[img_idx].timestamp, sizeof(struct timeval));
return TRUE;
}
}
@@ -334,7 +350,7 @@ struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev) {
* Free the image and enqueue the buffer (Thread safe)
* This must be done after processing the image, because else all buffers are locked
*/
void v4l2_image_free(struct v4l2_device *dev, struct v4l2_img_buf *img_buf)
void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
{
struct v4l2_buffer buf;
@@ -342,9 +358,9 @@ void v4l2_image_free(struct v4l2_device *dev, struct v4l2_img_buf *img_buf)
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = img_buf->idx;
buf.index = img->buf_idx;
if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
printf("[v4l2] Could not enqueue %d for %s\n", img_buf->idx, dev->name);
printf("[v4l2] Could not enqueue %d for %s\n", img->buf_idx, dev->name);
}
}
sw/airborne/modules/computer_vision/lib/v4l/v4l2.h
+6 -5
View File
@@ -28,16 +28,17 @@
#ifndef _CV_LIB_V4L2_H
#define _CV_LIB_V4L2_H
#include "std.h"
#include <linux/v4l2-subdev.h>
#include <pthread.h>
#include <sys/time.h>
#include "std.h"
#include "lib/vision/image.h"
#define V4L2_IMG_NONE 255 //< There currently no image available
/* V4L2 memory mapped image buffer */
struct v4l2_img_buf {
uint8_t idx; //< The index of the buffer
size_t length; //< The size of the buffer
struct timeval timestamp; //< The time value of the image
void *buf; //< Pointer to the memory mapped buffer
@@ -59,9 +60,9 @@ struct v4l2_device {
/* External functions */
bool_t v4l2_init_subdev(char *subdev_name, uint8_t pad, uint8_t which, uint16_t code, uint16_t width, uint16_t height);
struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt);
struct v4l2_img_buf *v4l2_image_get(struct v4l2_device *dev);
struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev);
void v4l2_image_free(struct v4l2_device *dev, struct v4l2_img_buf *img_buf);
void v4l2_image_get(struct v4l2_device *dev, struct image_t *img);
bool_t v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img);
void v4l2_image_free(struct v4l2_device *dev, struct image_t *img);
bool_t v4l2_start_capture(struct v4l2_device *dev);
bool_t v4l2_stop_capture(struct v4l2_device *dev);
void v4l2_close(struct v4l2_device *dev);
sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.c → sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
View File
sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.h → sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h
View File
sw/airborne/modules/computer_vision/lib/vision/image.c
+188
View File
@@ -0,0 +1,188 @@
/*
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/computer_vision/lib/vision/image.c
* Image helper functions, like resizing, color filter, converters...
*/
#include "image.h"
#include <stdlib.h>
#include <string.h>
/**
* Create a new image
* @param[out] *img The output image
* @param[in] width The width of the image
* @param[in] height The height of the image
* @param[in] type The type of image (YUV422 or grayscale)
*/
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
{
// Set the variables
img->type = type;
img->w = width;
img->h = height;
// Depending on the type the size differs
if(type == IMAGE_YUV422)
img->buf = malloc(sizeof(uint8_t)*2 * width * height);
else if(type == IMAGE_JPEG)
img->buf = malloc(sizeof(uint8_t)*1.1 * width * height); // At maximum quality this is enough
else
img->buf = malloc(sizeof(uint8_t) * width * height);
}
/**
* Free the image
* @param[in] *img The image to free
*/
void image_free(struct image_t *img)
{
free(img->buf);
}
/**
* Convert an image to grayscale.
* Depending on the output type the U/V bytes are removed
* @param[in] *input The input image (Needs to be YUV422)
* @param[out] *output The output image
*/
void image_to_grayscale(struct image_t *input, struct image_t *output)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
source++;
// Copy the creation timestamp (stays the same)
memcpy(&output->ts, &input->ts, sizeof(struct timeval));
// Copy the pixels
for (int y = 0; y < output->h; y++) {
for (int x = 0; x < output->w; x++) {
if(output->type == IMAGE_YUV422)
*dest++ = 127; // U / V
*dest++ = *source; // Y
source += 2;
}
}
}
/**
* Filter colors in an YUV422 image
* @param[in] *input The input image to filter
* @param[out] *output The filtered output image
* @param[in] y_m The Y minimum value
* @param[in] y_M The Y maximum value
* @param[in] u_m The U minimum value
* @param[in] u_M The U maximum value
* @param[in] v_m The V minimum value
* @param[in] v_M The V maximum value
* @return The amount of filtered pixels
*/
uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
uint8_t u_M, uint8_t v_m, uint8_t v_M)
{
uint16_t cnt = 0;
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
// Copy the creation timestamp (stays the same)
memcpy(&output->ts, &input->ts, sizeof(struct timeval));
// Go trough all the pixels
for (uint16_t y = 0; y < output->h; y++) {
for (uint16_t x = 0; x < output->w; x += 2) {
// Check if the color is inside the specified values
if (
(dest[1] >= y_m)
&& (dest[1] <= y_M)
&& (dest[0] >= u_m)
&& (dest[0] <= u_M)
&& (dest[2] >= v_m)
&& (dest[2] <= v_M)
) {
cnt ++;
// UYVY
dest[0] = 64; // U
dest[1] = source[1]; // Y
dest[2] = 255; // V
dest[3] = source[3]; // Y
} else {
// UYVY
char u = source[0] - 127;
u /= 4;
dest[0] = 127; // U
dest[1] = source[1]; // Y
u = source[2] - 127;
u /= 4;
dest[2] = 127; // V
dest[3] = source[3]; // Y
}
// Go to the next 2 pixels
dest += 4;
source += 4;
}
}
return cnt;
}
/**
* Simplified high-speed low CPU downsample function without averaging
* downsample factor must be 1, 2, 4, 8 ... 2^X
* image of typ UYVY expected. Only one color UV per 2 pixels
*
* we keep the UV color of the first pixel pair
* and sample the intensity evenly 1-3-5-7-... or 1-5-9-...
*
* input: u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=1 u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=2 u1y1v1 (skip2) y3 (skip2) u5y5v5 (skip2 y7 (skip2) ...
* downsample=4 u1y1v1 (skip6) y5 (skip6) ...
* @param[in] *input The input YUV422 image
* @param[out] *output The downscaled YUV422 image
* @param[in] downsample The downsampel facter (must be downsample=2^X)
*/
void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint16_t downsample)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
uint16_t pixelskip = (downsample - 1) * 2;
// Copy the creation timestamp (stays the same)
memcpy(&output->ts, &input->ts, sizeof(struct timeval));
// Go trough all the pixels
for (uint16_t y = 0; y < output->h; y++) {
for (uint16_t x = 0; x < output->w; x += 2) {
// YUYV
*dest++ = *source++; // U
*dest++ = *source++; // Y
*dest++ = *source++; // V
source += pixelskip;
*dest++ = *source++; // Y
source += pixelskip;
}
// read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first
source += (downsample-1) * input->w * 2;
}
}
sw/airborne/modules/computer_vision/lib/vision/image.h
+60
View File
@@ -0,0 +1,60 @@
/*
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/computer_vision/lib/vision/image.h
* Image helper functions like resizing, color filter, converters...
*/
#ifndef _CV_LIB_VISION_IMAGE_H
#define _CV_LIB_VISION_IMAGE_H
#include "std.h"
#include <sys/time.h>
/* The different type of images we currently support */
enum image_type {
IMAGE_YUV422, //< UYVY format
IMAGE_GRAYSCALE, //< Grayscale image with only the Y part
IMAGE_JPEG //< An JPEG encoded image
};
/* Main image structure */
struct image_t {
enum image_type type; //< The image type
uint16_t w; //< Image width
uint16_t h; //< Image height
struct timeval ts; //< The timestamp of creation
uint8_t buf_idx; //< Buffer index for V4L2 freeing
uint32_t buf_size; //< The buffer size
void *buf; //< Image buffer (depending on the image_type)
};
/* Usefull image functions */
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type);
void image_free(struct image_t *img);
void image_to_grayscale(struct image_t *input, struct image_t *output);
uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
uint8_t u_M, uint8_t v_m, uint8_t v_M);
void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint16_t downsample);
#endif
sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.c → sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c
+2 -2
View File
@@ -243,8 +243,8 @@ int calculateError(int *ImC, int width, int height)
return error;
}
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, uint16_t *p_x, uint16_t *p_y, uint16_t n_found_points,
uint16_t imW, uint16_t imH, uint16_t *new_x, uint16_t *new_y, bool_t *status, uint16_t half_window_size, uint8_t max_iterations)
{
// A straightforward one-level implementation of Lucas-Kanade.
// For all points:
sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.h → sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h
+4 -2
View File
@@ -27,6 +27,8 @@
#ifndef OPTIC_FLOW_INT_H
#define OPTIC_FLOW_INT_H
#include "std.h"
void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height);
void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height);
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size,
@@ -35,8 +37,8 @@ void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size);
int getSumPatch(int *Patch, int size);
int calculateG(int *G, int *DX, int *DY, int half_window_size);
int calculateError(int *ImC, int width, int height);
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, uint16_t *p_x, uint16_t *p_y, uint16_t n_found_points,
uint16_t imW, uint16_t imH, uint16_t *new_x, uint16_t *new_y, bool_t *status, uint16_t half_window_size, uint8_t max_iterations);
void quick_sort(float *a, int n);
void quick_sort_int(int *a, int n);
void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH);
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+62 -63
View File
@@ -36,8 +36,9 @@
#include "opticflow_calculator.h"
// Computer Vision
#include "cv/opticflow/lucas_kanade.h"
#include "cv/opticflow/fast_rosten.h"
#include "lib/vision/image.h"
#include "lib/vision/lucas_kanade.h"
#include "lib/vision/fast_rosten.h"
// ARDrone Vertical Camera Parameters
#define FOV_H 0.67020643276
@@ -56,6 +57,9 @@ static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishti
/**
* Initialize the opticflow calculator
* @param[out] *opticflow The new optical flow calculator
* @param[in] *w The image width
* @param[in] *h The image height
*/
void opticflow_calc_init(struct opticflow_t *opticflow, unsigned int w, unsigned int h)
{
@@ -75,21 +79,24 @@ void opticflow_calc_init(struct opticflow_t *opticflow, unsigned int w, unsigned
/**
* Run the optical flow on a new image frame
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
* @param[in] *state The state of the drone
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct v4l2_img_buf *img, struct opticflow_result_t *result)
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
{
// Corner Tracking
// Working Variables
int max_count = 25;
int borderx = 24, bordery = 24;
int x[MAX_COUNT], y[MAX_COUNT];
int new_x[MAX_COUNT], new_y[MAX_COUNT];
int status[MAX_COUNT];
uint16_t borderx = 24, bordery = 24;
uint16_t x[MAX_COUNT], y[MAX_COUNT];
uint16_t new_x[MAX_COUNT], new_y[MAX_COUNT];
bool_t status[MAX_COUNT];
int dx[MAX_COUNT], dy[MAX_COUNT];
// Update FPS for information
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->timestamp) / 1000.);
memcpy(&opticflow->prev_timestamp, &img->timestamp, sizeof(struct timeval));
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
if (!opticflow->got_first_img) {
CvtYUYV2Gray(opticflow->prev_gray_frame, img->buf, opticflow->img_w, opticflow->img_h);
@@ -101,51 +108,46 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// *************************************************************************************
// FAST corner detection
int fast_threshold = 20;
xyFAST *pnts_fast;
pnts_fast = fast9_detect((const byte *)opticflow->prev_gray_frame, opticflow->img_w, opticflow->img_h, opticflow->img_w,
int fast_threshold = 5;
xyFAST *pnts_fast = fast9_detect((const byte *)opticflow->prev_gray_frame, opticflow->img_w, opticflow->img_h, opticflow->img_w,
fast_threshold, &result->count);
if (result->count > MAX_COUNT) { result->count = MAX_COUNT; }
for (int i = 0; i < result->count; i++) {
x[i] = pnts_fast[i].x;
y[i] = pnts_fast[i].y;
// Copy the points and remove neighboring corners
const float min_distance2 = 10 * 10;
bool_t remove_points[MAX_COUNT];
uint16_t count_fil = 0;
memset(&remove_points, FALSE, sizeof(bool_t) * MAX_COUNT);
for (uint16_t i = 0; i < result->count; i++) {
if(remove_points[i])
continue;
x[count_fil] = pnts_fast[i].x;
y[count_fil++] = pnts_fast[i].y;
// Skip some if they are too close
for(uint16_t j = i+1; j < result->count; j++) {
float distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
if(distance2 < min_distance2)
remove_points[j] = TRUE;
}
}
free(pnts_fast);
// Remove neighboring corners
const float min_distance = 3;
float min_distance2 = min_distance * min_distance;
int labelmin[MAX_COUNT];
for (int i = 0; i < result->count; i++) {
for (int j = i + 1; j < result->count; j++) {
// distance squared:
float distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
if (distance2 < min_distance2) {
labelmin[i] = 1;
}
}
}
int count_fil = result->count;
for (int i = result->count - 1; i >= 0; i--) {
int remove_point = 0;
if (labelmin[i]) {
remove_point = 1;
}
if (remove_point) {
for (int c = i; c < count_fil - 1; c++) {
x[c] = x[c + 1];
y[c] = y[c + 1];
}
count_fil--;
}
}
if (count_fil > max_count) { count_fil = max_count; }
if(count_fil > 25) count_fil = 25;
result->count = count_fil;
uint8_t *im = (uint8_t *)img->buf;
for(int i = 0; i < result->count; i++) {
uint16_t idx = 2*y[i]*opticflow->img_w + 2*x[i];
im[idx] = 255;
idx = idx+1 % (opticflow->img_w*opticflow->img_h*2);
im[idx] = 255;
idx = idx+1 % (opticflow->img_w*opticflow->img_h*2);
im[idx] = 255;
}
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
@@ -154,16 +156,10 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
opticFlowLK(opticflow->gray_frame, opticflow->prev_gray_frame, x, y,
count_fil, opticflow->img_w, opticflow->img_h, new_x, new_y, status, 5, 100);
result->flow_count = count_fil;
for (int i = count_fil - 1; i >= 0; i--) {
int remove_point = 1;
if (status[i] && !(new_x[i] < borderx || new_x[i] > (opticflow->img_w - 1 - borderx) ||
new_y[i] < bordery || new_y[i] > (opticflow->img_h - 1 - bordery))) {
remove_point = 0;
}
if (remove_point) {
// Remove points if we lost tracking
/* for (int i = count_fil - 1; i >= 0; i--) {
if (!status[i] || new_x[i] < borderx || new_x[i] > (opticflow->img_w - 1 - borderx) ||
new_y[i] < bordery || new_y[i] > (opticflow->img_h - 1 - bordery)) {
for (int c = i; c < result->flow_count - 1; c++) {
x[c] = x[c + 1];
y[c] = y[c + 1];
@@ -172,7 +168,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
}
result->flow_count--;
}
}
}*/
result->dx_sum = 0.0;
result->dy_sum = 0.0;
@@ -196,7 +192,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
}
// Flow Derotation
result->diff_pitch = (state->theta - opticflow->prev_pitch) * opticflow->img_h / FOV_H;
/*result->diff_pitch = (state->theta - opticflow->prev_pitch) * opticflow->img_h / FOV_H;
result->diff_roll = (state->phi - opticflow->prev_roll) * opticflow->img_w / FOV_W;
opticflow->prev_pitch = state->theta;
opticflow->prev_roll = state->phi;
@@ -242,12 +238,15 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
*/
memcpy(opticflow->prev_gray_frame, opticflow->gray_frame, opticflow->img_w * opticflow->img_h);
}
/**
* calculate the difference from start till finish
* Calculate the difference from start till finish
* @param[in] *starttime The start time to calculate the difference from
* @param[in] *finishtime The finish time to calculate the difference from
* @return The difference in milliseconds
*/
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime)
{
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+1 -1
View File
@@ -52,6 +52,6 @@ struct opticflow_t
void opticflow_calc_init(struct opticflow_t *opticflow, unsigned int w, unsigned int h);
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct v4l2_img_buf *img, struct opticflow_result_t *result);
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result);
#endif /* OPTICFLOW_CALCULATOR_H */
sw/airborne/modules/computer_vision/opticflow_module.c
+44 -3
View File
@@ -32,7 +32,9 @@
#include <pthread.h>
#include "state.h"
#include "subsystems/abi.h"
#include "lib/v4l/v4l2.h"
#include "lib/encoding/jpeg.h"
/* default sonar/agl to use in opticflow visual_estimator */
#ifndef OPTICFLOW_AGL_ID
@@ -139,10 +141,17 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
return 0;
}
#ifdef OPTICFLOW_DEBUG
// Create a new JPEG image
struct image_t img_jpeg;
image_create(&img_jpeg, opticflow_dev->w, opticflow_dev->h, IMAGE_JPEG);
#endif
/* Main loop of the optical flow calculation */
while(TRUE) {
// Try to fetch an image
struct v4l2_img_buf *img = v4l2_image_get(opticflow_dev);
struct image_t img;
v4l2_image_get(opticflow_dev, &img);
// Copy the state
pthread_mutex_lock(&opticflow_mutex);
@@ -152,7 +161,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
// Do the optical flow calculation
struct opticflow_result_t temp_result;
opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result);
opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
@@ -160,9 +169,41 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
opticflow_got_result = TRUE;
pthread_mutex_unlock(&opticflow_mutex);
#ifdef OPTICFLOW_DEBUG
jpeg_encode_image(&img, &img_jpeg, 99, TRUE);
// Open process to send using netcat (in a fork because sometimes kills itself???)
pid_t pid = fork();
if(pid < 0) {
printf("[viewvideo] Could not create netcat fork.\n");
}
else if(pid ==0) {
// We are the child and want to send the image
FILE *netcat = popen("nc 192.168.1.2 5000 2>/dev/null", "w");
if (netcat != NULL) {
fwrite(img_jpeg.buf, sizeof(uint8_t), img_jpeg.buf_size, netcat);
pclose(netcat); // Ignore output, because it is too much when not connected
} else {
printf("[viewvideo] Failed to open netcat process.\n");
}
// Exit the program since we don't want to continue after transmitting
exit(0);
}
else {
// We want to wait until the child is finished
wait(NULL);
}
#endif
// Free the image
v4l2_image_free(opticflow_dev, img);
v4l2_image_free(opticflow_dev, &img);
}
#ifdef OPTICFLOW_DEBUG
image_free(&img_jpeg);
#endif
}
/**
sw/airborne/modules/computer_vision/viewvideo.c
+26 -36
View File
@@ -42,9 +42,9 @@
// Video
#include "lib/v4l/v4l2.h"
#include "cv/resize.h"
#include "cv/encoding/jpeg.h"
#include "cv/encoding/rtp.h"
#include "lib/vision/image.h"
#include "lib/encoding/jpeg.h"
#include "lib/encoding/rtp.h"
// Threaded computer vision
#include <pthread.h>
@@ -136,19 +136,17 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
}
// Resize image if needed
struct img_struct small;
small.w = viewvideo.dev->w / viewvideo.downsize_factor;
small.h = viewvideo.dev->h / viewvideo.downsize_factor;
if (viewvideo.downsize_factor != 1) {
small.buf = (uint8_t *)malloc(small.w * small.h * 2);
} else {
small.buf = NULL;
}
struct image_t img_small;
image_create(&img_small,
viewvideo.dev->w/viewvideo.downsize_factor,
viewvideo.dev->h/viewvideo.downsize_factor,
IMAGE_YUV422);
// JPEG compression (8.25 bits are required for a 100% quality image, margin of ~0.55)
uint8_t *jpegbuf = (uint8_t *)malloc(ceil(small.w * small.h * 1.1));
// Create the JPEG encoded image
struct image_t img_jpeg;
image_create(&img_jpeg, img_small.w, img_small.h, IMAGE_JPEG);
// time
// Initialize timing
uint32_t microsleep = (uint32_t)(1000000. / (float)viewvideo.fps);
struct timeval last_time;
gettimeofday(&last_time, NULL);
@@ -169,14 +167,15 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
last_time = vision_thread_sleep_time;
// Wait for a new frame (blocking)
struct v4l2_img_buf *img = v4l2_image_get(viewvideo.dev);
struct image_t img;
v4l2_image_get(viewvideo.dev, &img);
// Check if we need to take a shot
if (viewvideo.take_shot) {
// Create a high quality image (99% JPEG encoded)
uint8_t *jpegbuf_hr = (uint8_t *)malloc(ceil(viewvideo.dev->w * viewvideo.dev->h * 1.1));
uint8_t *end = jpeg_encode_image(img->buf, jpegbuf_hr, 99, FOUR_TWO_TWO, viewvideo.dev->w, viewvideo.dev->h, TRUE);
uint32_t size = end - (jpegbuf_hr);
struct image_t jpeg_hr;
image_create(&jpeg_hr, img.w, img.h, IMAGE_JPEG);
jpeg_encode_image(&img, &jpeg_hr, 99, TRUE);
// Search for a file where we can write to
char save_name[128];
@@ -189,7 +188,7 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
printf("[viewvideo-thread] Could not write shot %s.\n", save_name);
} else {
// Save it to the file and close it
fwrite(jpegbuf_hr, sizeof(uint8_t), size, fp);
fwrite(jpeg_hr.buf, sizeof(uint8_t), jpeg_hr.buf_size, fp);
fclose(fp);
}
@@ -199,25 +198,18 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
}
// We finished the shot
free(jpegbuf_hr);
image_free(&jpeg_hr);
viewvideo.take_shot = FALSE;
}
// Only resize when needed
if (viewvideo.downsize_factor != 1) {
struct img_struct input;
input.buf = img->buf;
input.w = viewvideo.dev->w;
input.h = viewvideo.dev->h;
resize_uyuv(&input, &small, viewvideo.downsize_factor);
image_yuv422_downsample(&img, &img_small, viewvideo.downsize_factor);
jpeg_encode_image(&img_small, &img_jpeg, VIEWVIDEO_QUALITY_FACTOR, VIEWVIDEO_USE_NETCAT);
} else {
small.buf = img->buf;
jpeg_encode_image(&img, &img_jpeg, VIEWVIDEO_QUALITY_FACTOR, VIEWVIDEO_USE_NETCAT);
}
// JPEG encode the image:
uint8_t *end = jpeg_encode_image(small.buf, jpegbuf, VIEWVIDEO_QUALITY_FACTOR, FOUR_TWO_TWO, small.w, small.h, VIEWVIDEO_USE_NETCAT);
uint32_t size = end - (jpegbuf);
#if VIEWVIDEO_USE_NETCAT
// Open process to send using netcat (in a fork because sometimes kills itself???)
pid_t pid = fork();
@@ -246,8 +238,7 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
// Send image with RTP
rtp_frame_send(
&VIEWVIDEO_DEV, // UDP device
jpegbuf, size, // JPEG
small.w, small.h, // Img Size
&img_jpeg,
0, // Format 422
VIEWVIDEO_QUALITY_FACTOR, // Jpeg-Quality
0, // DRI Header
@@ -264,13 +255,12 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
#endif
// Free the image
v4l2_image_free(viewvideo.dev, img);
v4l2_image_free(viewvideo.dev, &img);
}
// Free all buffers
free(jpegbuf);
if (viewvideo.downsize_factor != 1)
free(small.buf);
image_free(&img_jpeg);
image_free(&img_small);
return 0;
}