mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
49d08ba72a
Minlure is a port of ArduPilot to Minnow Board connected to daughter board. Very few of those were produced and nobody is flying with it. It served its purpose and all the the improvements to ArduPilot remain regardless of it not being supported anymore. Now it's just adding maintenance work with no clear benefit, so pull the plug.
355 lines
13 KiB
C++
355 lines
13 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
|
* Author: Petri Tanskanen <tpetri@inf.ethz.ch>
|
|
* Lorenz Meier <lm@inf.ethz.ch>
|
|
* Samuel Zihlmann <samuezih@ee.ethz.ch>
|
|
*
|
|
* Modified to fit the APM framework by:
|
|
* Julien BERAUD <julien.beraud@parrot.com>
|
|
*
|
|
* 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 <AP_HAL/AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
|
|
#include "Flow_PX4.h"
|
|
|
|
#include <cmath>
|
|
#include <stdio.h>
|
|
#include <stdlib.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 = ceilf(((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 threshold */
|
|
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
|