diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 71bbf51f43..199e3c6462 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -45,8 +45,9 @@ - + + + @@ -55,13 +56,14 @@ - - + + + diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml index 1abd3caf18..98f8a5935c 100644 --- a/conf/modules/video_rtp_stream.xml +++ b/conf/modules/video_rtp_stream.xml @@ -37,8 +37,9 @@ - - + + + diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h deleted file mode 100644 index 46ffdb413a..0000000000 --- a/sw/airborne/modules/computer_vision/cv/color.h +++ /dev/null @@ -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 -#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; -} - diff --git a/sw/airborne/modules/computer_vision/cv/image.h b/sw/airborne/modules/computer_vision/cv/image.h deleted file mode 100644 index cf749ced5b..0000000000 --- a/sw/airborne/modules/computer_vision/cv/image.h +++ /dev/null @@ -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 diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h deleted file mode 100644 index b0524a5270..0000000000 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ /dev/null @@ -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 -#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; - } -} - diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c similarity index 97% rename from sw/airborne/modules/computer_vision/cv/encoding/jpeg.c rename to sw/airborne/modules/computer_vision/lib/encoding/jpeg.c index e26c69e900..c0ede9eebc 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c @@ -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) diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.h similarity index 79% rename from sw/airborne/modules/computer_vision/cv/encoding/jpeg.h rename to sw/airborne/modules/computer_vision/lib/encoding/jpeg.h index f36021f15e..fafe6dbc9f 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h +++ b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.h @@ -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); diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/lib/encoding/rtp.c similarity index 94% rename from sw/airborne/modules/computer_vision/cv/encoding/rtp.c rename to sw/airborne/modules/computer_vision/lib/encoding/rtp.c index 92961f6ff1..047e574d52 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/lib/encoding/rtp.c @@ -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++; } diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h b/sw/airborne/modules/computer_vision/lib/encoding/rtp.h similarity index 92% rename from sw/airborne/modules/computer_vision/cv/encoding/rtp.h rename to sw/airborne/modules/computer_vision/lib/encoding/rtp.h index 7452153627..d099d1a1d8 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h +++ b/sw/airborne/modules/computer_vision/lib/encoding/rtp.h @@ -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? diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c deleted file mode 100644 index fba489e282..0000000000 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.c +++ /dev/null @@ -1,104 +0,0 @@ -#include "socket.h" - -#include -#include -#include -#include -#include - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -# 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 -//#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; -} diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.h b/sw/airborne/modules/computer_vision/lib/udp/socket.h deleted file mode 100644 index 98a831a6ee..0000000000 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef SOCKET_H -#define SOCKET_H - - -#include -#include - -#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 */ - diff --git a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c index 576a4e097a..077e4dfc61 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c @@ -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); } } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h index 7c83e44da9..4b900056c3 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h @@ -28,16 +28,17 @@ #ifndef _CV_LIB_V4L2_H #define _CV_LIB_V4L2_H -#include "std.h" #include #include #include +#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); diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.c b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c similarity index 100% rename from sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.c rename to sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.h b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h similarity index 100% rename from sw/airborne/modules/computer_vision/cv/opticflow/fast_rosten.h rename to sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c new file mode 100644 index 0000000000..086c3d195c --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -0,0 +1,188 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 +#include + +/** + * 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; + } +} diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h new file mode 100644 index 0000000000..8dd8054fc4 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 + +/* 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 diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c similarity index 98% rename from sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.c rename to sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c index f27f614cb0..af7ed226d2 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.c +++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c @@ -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: diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.h b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h similarity index 88% rename from sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.h rename to sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h index 192b5a1bbc..9d1be401bd 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/lucas_kanade.h +++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.h @@ -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); diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index ce8086abfc..43d015c0fa 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -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) { diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 83d9ed4b04..a8d9538a10 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -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 */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index dcba3bb6b4..bd85342526 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -32,7 +32,9 @@ #include #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 } /** diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 85ebd24e3e..b79f098711 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -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 @@ -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; }