zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
master
Julien BERAUD 9 years ago committed by Andrew Tridgell
parent
c75704bde3
commit
0476e698af
2 changed files with 407 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 368
      libraries/AP_HAL_Linux/Flow_PX4.cpp
  2. 39
      libraries/AP_HAL_Linux/Flow_PX4.h

368
libraries/AP_HAL_Linux/Flow_PX4.cpp
Unescape View File

@ -0,0 +1,368 @@
/****************************************************************************
*
* 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 <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#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

39
libraries/AP_HAL_Linux/Flow_PX4.h
Unescape View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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
Write Preview
Loading…
Cancel
Save