From 0476e698af399e71eac796fcc5c25b7eafe441ba Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Tue, 24 Nov 2015 09:54:49 +0100 Subject: [PATCH] AP_HAL_Linux: Flow_PX4 interface Implementation of the PX4 flow algorithm for ardupilot. Based on the original PX4 Flow code, it has diverged a lot. I have kept the license header since it is required. I have removed all the unused and dead code on current implementation, modified the code to make it clearer, re-indented it and changed the way some params are calculated. It has been tested on PC and on board and showed results that I assumed were OK. No optical flow Loiter tests have been undertaken since it requires a Sonar which will be added soon. Limitations : Some parts were written in ARM assembly and I rewrote them very naively to get them to be more easily portable. A simple optimisation would be to re-introduce assembly code for ARM as a separate asm file with methods for fixed resolutions that would reduce a lot the amount of calculation and memory read/writes. Then writing a version in NEON assembly would even be more optimised and then maybe an Intel version. --- libraries/AP_HAL_Linux/Flow_PX4.cpp | 368 ++++++++++++++++++++++++++++ libraries/AP_HAL_Linux/Flow_PX4.h | 39 +++ 2 files changed, 407 insertions(+) create mode 100644 libraries/AP_HAL_Linux/Flow_PX4.cpp create mode 100644 libraries/AP_HAL_Linux/Flow_PX4.h diff --git a/libraries/AP_HAL_Linux/Flow_PX4.cpp b/libraries/AP_HAL_Linux/Flow_PX4.cpp new file mode 100644 index 0000000000..9c689060f7 --- /dev/null +++ b/libraries/AP_HAL_Linux/Flow_PX4.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Samuel Zihlmann + * + * Modified to fit the APM framework by: + * Julien BERAUD + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +#include +#include +#include +#include +#include "Flow_PX4.h" + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +Flow_PX4::Flow_PX4(uint32_t width, uint32_t bytesperline, + uint32_t max_flow_pixel, + float bottom_flow_feature_threshold, + float bottom_flow_value_threshold) : + _width(width), + _bytesperline(bytesperline), + _search_size(max_flow_pixel), + _bottom_flow_feature_threshold(bottom_flow_feature_threshold), + _bottom_flow_value_threshold(bottom_flow_value_threshold) +{ + /* _pixlo is _search_size + 1 because if we need to evaluate + * the subpixels up/left of the first pixel, the index + * will be equal to _pixlo - _search_size -1 + * idem if we need to evaluate the subpixels down/right + * the index will be equal to _pixhi + _search_size + 1 + * which needs to remain inferior to _width - 1 + */ + _pixlo = _search_size + 1; + _pixhi = _width - 1 - (_search_size + 1); + /* 1 block is of size 2*_search_size + 1 + 1 pixel on each + * side for subpixel calculation. + * So _num_blocks = _width / (2 * _search_size + 3) + */ + _num_blocks = _width / (2 * _search_size + 3); + _pixstep = ceil(((float)(_pixhi - _pixlo)) / _num_blocks); +} + +/** + * @brief Compute the average pixel gradient of all horizontal and vertical + * steps + * + * @param image ... + * @param offX x coordinate of upper left corner of 8x8 pattern in image + * @param offY y coordinate of upper left corner of 8x8 pattern in image + */ +static inline uint32_t compute_diff(uint8_t *image, uint16_t offx, uint16_t offy, + uint16_t row_size, uint8_t window_size) +{ + /* calculate position in image buffer */ + /* we calc only the 4x4 pattern */ + uint16_t off = (offy + 2) * row_size + (offx + 2); + uint32_t acc = 0; + unsigned int i; + + for (i = 0; i < window_size; i++) { + /* accumulate differences between line1/2, 2/3, 3/4 for 4 pixels + * starting at offset off + */ + acc += abs(image[off + i] - image[off + i + row_size]); + acc += abs(image[off + i + row_size] - image[off + i + 2 * row_size]); + acc += abs(image[off + i + 2 * row_size] - + image[off + i + 3 * row_size]); + + /* accumulate differences between col1/2, 2/3, 3/4 for 4 pixels starting + * at off + */ + acc += abs(image[off + row_size * i] - image[off + row_size * i + 1]); + acc += abs(image[off + row_size * i + 1] - + image[off + row_size * i + 2]); + acc += abs(image[off + row_size * i + 2] - + image[off + row_size * i + 3]); + } + + return acc; +} + +/** + * @brief Compute SAD of two pixel windows. + * + * @param image1 ... + * @param image2 ... + * @param off1X x coordinate of upper left corner of pattern in image1 + * @param off1Y y coordinate of upper left corner of pattern in image1 + * @param off2X x coordinate of upper left corner of pattern in image2 + * @param off2Y y coordinate of upper left corner of pattern in image2 + */ +static inline uint32_t compute_sad(uint8_t *image1, uint8_t *image2, + uint16_t off1x, uint16_t off1y, + uint16_t off2x, uint16_t off2y, + uint16_t row_size, uint16_t window_size) +{ + /* calculate position in image buffer + * off1 for image1 and off2 for image2 + */ + uint16_t off1 = off1y * row_size + off1x; + uint16_t off2 = off2y * row_size + off2x; + unsigned int i,j; + uint32_t acc = 0; + + for (i = 0; i < window_size; i++) { + for (j = 0; j < window_size; j++) { + acc += abs(image1[off1 + i + j*row_size] - + image2[off2 + i + j*row_size]); + } + } + return acc; +} + +/** + * @brief Compute SAD distances of subpixel shift of two pixel patterns. + * + * @param image1 ... + * @param image2 ... + * @param off1X x coordinate of upper left corner of pattern in image1 + * @param off1Y y coordinate of upper left corner of pattern in image1 + * @param off2X x coordinate of upper left corner of pattern in image2 + * @param off2Y y coordinate of upper left corner of pattern in image2 + * @param acc array to store SAD distances for shift in every direction + */ +static inline uint32_t compute_subpixel(uint8_t *image1, uint8_t *image2, + uint16_t off1x, uint16_t off1y, + uint16_t off2x, uint16_t off2y, + uint32_t *acc, uint16_t row_size, + uint16_t window_size) +{ + /* calculate position in image buffer */ + uint16_t off1 = off1y * row_size + off1x; // image1 + uint16_t off2 = off2y * row_size + off2x; // image2 + uint8_t sub[8]; + uint16_t i, j, k; + + memset(acc, 0, window_size * sizeof(uint32_t)); + + for (i = 0; i < window_size; i++) + { + for (j = 0; j < window_size; j++) { + /* the 8 s values are from following positions for each pixel (X): + * + - + - + - + + * + 5 7 + + * + - + 6 + - + + * + 4 X 0 + + * + - + 2 + - + + * + 3 1 + + * + - + - + - + + */ + + /* subpixel 0 is the mean value of base pixel and + * the pixel on the right, subpixel 1 is the mean + * value of base pixel, the pixel on the right, + * the pixel down from it, and the pixel down on + * the right. etc... + */ + sub[0] = (image2[off2 + i + j*row_size] + + image2[off2 + i + 1 + j*row_size])/2; + + sub[1] = (image2[off2 + i + j*row_size] + + image2[off2 + i + 1 + j*row_size] + + image2[off2 + i + (j+1)*row_size] + + image2[off2 + i + 1 + (j+1)*row_size])/4; + + sub[2] = (image2[off2 + i + j*row_size] + + image2[off2 + i + 1 + (j+1)*row_size])/2; + + sub[3] = (image2[off2 + i + j*row_size] + + image2[off2 + i - 1 + j*row_size] + + image2[off2 + i - 1 + (j+1)*row_size] + + image2[off2 + i + (j+1)*row_size])/4; + + sub[4] = (image2[off2 + i + j*row_size] + + image2[off2 + i - 1 + (j+1)*row_size])/2; + + sub[5] = (image2[off2 + i + j*row_size] + + image2[off2 + i - 1 + j*row_size] + + image2[off2 + i - 1 + (j-1)*row_size] + + image2[off2 + i + (j-1)*row_size])/4; + + sub[6] = (image2[off2 + i + j*row_size] + + image2[off2 + i + (j-1)*row_size])/2; + + sub[7] = (image2[off2 + i + j*row_size] + + image2[off2 + i + 1 + j*row_size] + + image2[off2 + i + (j-1)*row_size] + + image2[off2 + i + 1 + (j-1)*row_size])/4; + + for (k = 0; k < 8; k++) { + acc[k] += abs(image1[off1 + i + j*row_size] - sub[k]); + } + } + } + + return 0; +} + +uint8_t Flow_PX4::compute_flow(uint8_t *image1, uint8_t *image2, + uint32_t delta_time, float *pixel_flow_x, + float *pixel_flow_y) +{ + /* constants */ + const int16_t winmin = -_search_size; + const int16_t winmax = _search_size; + uint16_t i, j; + uint32_t acc[2*_search_size]; + int8_t dirsx[_num_blocks*_num_blocks]; + int8_t dirsy[_num_blocks*_num_blocks]; + uint8_t subdirs[_num_blocks*_num_blocks]; + float meanflowx = 0.0f; + float meanflowy = 0.0f; + uint16_t meancount = 0; + float histflowx = 0.0f; + float histflowy = 0.0f; + + /* iterate over all patterns + */ + for (j = _pixlo; j < _pixhi; j += _pixstep) + { + for (i = _pixlo; i < _pixhi; i += _pixstep) + { + /* test pixel if it is suitable for flow tracking */ + uint32_t diff = compute_diff(image1, i, j, (uint16_t) _bytesperline, + _search_size); + if (diff < _bottom_flow_feature_threshold) + { + continue; + } + + uint32_t dist = 0xFFFFFFFF; // set initial distance to "infinity" + int8_t sumx = 0; + int8_t sumy = 0; + int8_t ii, jj; + + for (jj = winmin; jj <= winmax; jj++) + { + for (ii = winmin; ii <= winmax; ii++) + { + uint32_t temp_dist = compute_sad(image1, image2, i, j, + i + ii, j + jj, + (uint16_t) _bytesperline, + 2 * _search_size); + if (temp_dist < dist) + { + sumx = ii; + sumy = jj; + dist = temp_dist; + } + } + } + + /* acceptance SAD distance threshhold */ + if (dist < _bottom_flow_value_threshold) + { + meanflowx += (float) sumx; + meanflowy += (float) sumy; + + compute_subpixel(image1, image2, i, j, i + sumx, j + sumy, + acc, (uint16_t) _bytesperline, + 2 * _search_size); + uint32_t mindist = dist; // best SAD until now + uint8_t mindir = 8; // direction 8 for no direction + for(uint8_t k = 0; k < 2 * _search_size; k++) + { + if (acc[k] < mindist) + { + // SAD becomes better in direction k + mindist = acc[k]; + mindir = k; + } + } + dirsx[meancount] = sumx; + dirsy[meancount] = sumy; + subdirs[meancount] = mindir; + meancount++; + } + } + } + + /* evaluate flow calculation */ + if (meancount > _num_blocks*_num_blocks/2) + { + meanflowx /= meancount; + meanflowy /= meancount; + + /* use average of accepted flow values */ + uint32_t meancount_x = 0; + uint32_t meancount_y = 0; + + for (uint16_t h = 0; h < meancount; h++) + { + float subdirx = 0.0f; + if (subdirs[h] == 0 || subdirs[h] == 1 || subdirs[h] == 7) { + subdirx = 0.5f; + } + if (subdirs[h] == 3 || subdirs[h] == 4 || subdirs[h] == 5) { + subdirx = -0.5f; + } + histflowx += (float)dirsx[h] + subdirx; + meancount_x++; + + float subdiry = 0.0f; + if (subdirs[h] == 5 || subdirs[h] == 6 || subdirs[h] == 7) { + subdiry = -0.5f; + } + if (subdirs[h] == 1 || subdirs[h] == 2 || subdirs[h] == 3) { + subdiry = 0.5f; + } + histflowy += (float)dirsy[h] + subdiry; + meancount_y++; + } + + histflowx /= meancount_x; + histflowy /= meancount_y; + + *pixel_flow_x = histflowx; + *pixel_flow_y = histflowy; + } + else + { + *pixel_flow_x = 0.0f; + *pixel_flow_y = 0.0f; + return 0; + } + + /* calc quality */ + uint8_t qual = (uint8_t)(meancount * 255 / (_num_blocks*_num_blocks)); + + return qual; +} + +#endif diff --git a/libraries/AP_HAL_Linux/Flow_PX4.h b/libraries/AP_HAL_Linux/Flow_PX4.h new file mode 100644 index 0000000000..5e544f0170 --- /dev/null +++ b/libraries/AP_HAL_Linux/Flow_PX4.h @@ -0,0 +1,39 @@ +/* + This program 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 3 of the License, or + (at your option) any later version. + + This program 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 this program. If not, see . + */ +#ifndef __FLOW_PX4_H__ +#define __FLOW_PX4_H__ + +#include "AP_HAL_Linux.h" + +class Linux::Flow_PX4 { +public: + Flow_PX4(uint32_t width, uint32_t bytesperline, + uint32_t max_flow_pixel, + float bottom_flow_feature_threshold, + float bottom_flow_value_threshold); + uint8_t compute_flow(uint8_t *image1, uint8_t *image2, uint32_t delta_time, + float *pixel_flow_x, float *pixel_flow_y); +private: + uint32_t _width; + uint32_t _search_size; + uint32_t _bytesperline; + float _bottom_flow_feature_threshold; + float _bottom_flow_value_threshold; + uint16_t _pixlo; + uint16_t _pixhi; + uint16_t _pixstep; + uint8_t _num_blocks; +}; +#endif