mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 06:15:43 +08:00
Reverted to original ardrone2 makefile. Used astyle formating.
This commit is contained in:
hrvoje
+4
-24
@@ -19,31 +19,11 @@
|
||||
# <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
include $(PAPARAZZI_SRC)/conf/Makefile.arm-linux-toolchain
|
||||
#
|
||||
# This Makefile uses the generic Makefile.arm-linux and adds upload rules for the ARDrone2
|
||||
#
|
||||
|
||||
# Define OMAP A8 ARDrone2 specifc flags
|
||||
|
||||
FLOAT_ABI ?= -mfloat-abi=softfp
|
||||
#FLOAT_ABI ?= -mfloat-abi=hard
|
||||
|
||||
# ARCH_CFLAGS ?= -O3 -marm -funsafe-math-optimizations -mcpu=cortex-a8 -mtune=cortex-a8 -march=armv7-a -mfpu=neon -ftree-vectorize -ftree-vectorizer-verbose=1 -ffast-math -fomit-frame-pointer -funroll-loops
|
||||
ARCH_CFLAGS ?= -O3 -marm -mstructure-size-boundary=32 -funsafe-math-optimizations -mcpu=cortex-a8 -mtune=cortex-a8 -march=armv7-a -mfpu=neon -ftree-vectorize -ftree-vectorizer-verbose=1 -funroll-loops -ffast-math
|
||||
# When -pg is used this cannot -fomit-frame-pointer
|
||||
|
||||
# -ffast-math is added in generic Makefile.linux later on..
|
||||
|
||||
# TODO: arm{?}strip -R .comment -R .note -R .note.ABI-tag ap.elf
|
||||
|
||||
# with armv7, unaligned data can still be read
|
||||
CFLAGS += -DPPRZLINK_UNALIGNED_ACCESS=1
|
||||
|
||||
# add ARM specifc flags to CFLAGS, LDFLAGS
|
||||
CFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS) -pg
|
||||
LDFLAGS += $(FLOAT_ABI) -pg
|
||||
CXXFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS) -pg
|
||||
|
||||
# include the common linux Makefile (common CFLAGS, actual targets)
|
||||
include $(PAPARAZZI_SRC)/conf/Makefile.linux
|
||||
include $(PAPARAZZI_SRC)/conf/Makefile.arm-linux
|
||||
|
||||
DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/ardrone2.py
|
||||
|
||||
|
||||
@@ -240,36 +240,40 @@ void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint
|
||||
* @param[in] *input - input image (grayscale only)
|
||||
* @param[out] *output - the output image
|
||||
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
*/
|
||||
void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
|
||||
{
|
||||
// Create padded image based on input
|
||||
image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
|
||||
// Create padded image based on input
|
||||
image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
|
||||
|
||||
uint8_t *input_buf = (uint8_t *)input->buf;
|
||||
uint8_t *output_buf = (uint8_t *)output->buf;
|
||||
uint8_t *input_buf = (uint8_t *)input->buf;
|
||||
uint8_t *output_buf = (uint8_t *)output->buf;
|
||||
|
||||
// Skip first `border_size` rows, iterate through next input->h rows
|
||||
for (uint16_t i = border_size; i != (output->h - border_size); i++){
|
||||
// Skip first `border_size` rows, iterate through next input->h rows
|
||||
for (uint16_t i = border_size; i != (output->h - border_size); i++) {
|
||||
|
||||
// Mirror first `border_size` columns
|
||||
for (uint8_t j = 0; j != border_size; j++)
|
||||
output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
|
||||
// Mirror first `border_size` columns
|
||||
for (uint8_t j = 0; j != border_size; j++) {
|
||||
output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
|
||||
}
|
||||
|
||||
// Copy corresponding row values from input image
|
||||
memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
|
||||
// Copy corresponding row values from input image
|
||||
memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
|
||||
|
||||
// Mirror last `border_size` columns
|
||||
for (uint8_t j = 0; j != border_size; j++)
|
||||
output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size -1 - j];
|
||||
}
|
||||
// Mirror last `border_size` columns
|
||||
for (uint8_t j = 0; j != border_size; j++) {
|
||||
output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size - 1 - j];
|
||||
}
|
||||
}
|
||||
|
||||
// Mirror first `border_size` and last `border_size` rows
|
||||
for (uint8_t i = 0; i != border_size; i++){
|
||||
memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w], sizeof(uint8_t) * output->w);
|
||||
memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w], &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
|
||||
}
|
||||
// Mirror first `border_size` and last `border_size` rows
|
||||
for (uint8_t i = 0; i != border_size; i++) {
|
||||
memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w],
|
||||
sizeof(uint8_t) * output->w);
|
||||
memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w],
|
||||
&output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -281,37 +285,43 @@ void image_add_border(struct image_t *input, struct image_t *output, uint8_t bor
|
||||
* @param[in] *input - input image (grayscale only)
|
||||
* @param[out] *output - the output image
|
||||
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
*/
|
||||
void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
|
||||
{
|
||||
// Create output image, new image size is half the size of input image without padding (border)
|
||||
image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size ) / 2, input->type);
|
||||
// Create output image, new image size is half the size of input image without padding (border)
|
||||
image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size) / 2, input->type);
|
||||
|
||||
uint8_t *input_buf = (uint8_t *)input->buf;
|
||||
uint8_t *output_buf = (uint8_t *)output->buf;
|
||||
uint8_t *input_buf = (uint8_t *)input->buf;
|
||||
uint8_t *output_buf = (uint8_t *)output->buf;
|
||||
|
||||
uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
|
||||
uint16_t w = input->w;
|
||||
int32_t sum = 0;
|
||||
uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
|
||||
uint16_t w = input->w;
|
||||
int32_t sum = 0;
|
||||
|
||||
for (uint16_t i = 0; i != output->h; i++){
|
||||
for (uint16_t i = 0; i != output->h; i++) {
|
||||
|
||||
for (uint16_t j = 0; j != output->w; j++){
|
||||
row = border_size + 2 * i; // First skip border, then every second pixel
|
||||
col = border_size + 2 * j;
|
||||
for (uint16_t j = 0; j != output->w; j++) {
|
||||
row = border_size + 2 * i; // First skip border, then every second pixel
|
||||
col = border_size + 2 * j;
|
||||
|
||||
sum = 39 * ( input_buf[(row -2)*w + (col -2)] + input_buf[(row -2)*w + (col +2)] + input_buf[(row +2)*w + (col -2)] + input_buf[(row +2)*w + (col +2)]);
|
||||
sum += 156 * ( input_buf[(row -2)*w + (col -1)] + input_buf[(row -2)*w + (col +1)] + input_buf[(row -1)*w + (col +2)] + input_buf[(row +1)*w + (col -2)]
|
||||
+ input_buf[(row +1)*w + (col +2)] + input_buf[(row +2)*w + (col -1)] + input_buf[(row +2)*w + (col +1)] + input_buf[(row -1)*w + (col -2)]);
|
||||
sum += 234 * ( input_buf[(row -2)*w + (col)] + input_buf[(row)*w + (col -2)] + input_buf[(row)*w + (col +2)] + input_buf[(row +2)*w + (col)]);
|
||||
sum += 625 * ( input_buf[(row -1)*w + (col -1)] + input_buf[(row -1)*w + (col +1)] + input_buf[(row +1)*w + (col -1)] + input_buf[(row +1)*w + (col +1)]);
|
||||
sum += 938 * ( input_buf[(row -1)*w + (col)] + input_buf[(row)*w + (col -1)] + input_buf[(row)*w + (col +1)] + input_buf[(row +1)*w + (col)]);
|
||||
sum += 1406 * input_buf[(row)*w + (col)];
|
||||
sum = 39 * (input_buf[(row - 2) * w + (col - 2)] + input_buf[(row - 2) * w + (col + 2)] +
|
||||
input_buf[(row + 2) * w + (col - 2)] + input_buf[(row + 2) * w + (col + 2)]);
|
||||
sum += 156 * (input_buf[(row - 2) * w + (col - 1)] + input_buf[(row - 2) * w + (col + 1)] +
|
||||
input_buf[(row - 1) * w + (col + 2)] + input_buf[(row + 1) * w + (col - 2)]
|
||||
+ input_buf[(row + 1) * w + (col + 2)] + input_buf[(row + 2) * w + (col - 1)] + input_buf[(row + 2) * w + (col + 1)] +
|
||||
input_buf[(row - 1) * w + (col - 2)]);
|
||||
sum += 234 * (input_buf[(row - 2) * w + (col)] + input_buf[(row) * w + (col - 2)] +
|
||||
input_buf[(row) * w + (col + 2)] + input_buf[(row + 2) * w + (col)]);
|
||||
sum += 625 * (input_buf[(row - 1) * w + (col - 1)] + input_buf[(row - 1) * w + (col + 1)] +
|
||||
input_buf[(row + 1) * w + (col - 1)] + input_buf[(row + 1) * w + (col + 1)]);
|
||||
sum += 938 * (input_buf[(row - 1) * w + (col)] + input_buf[(row) * w + (col - 1)] +
|
||||
input_buf[(row) * w + (col + 1)] + input_buf[(row + 1) * w + (col)]);
|
||||
sum += 1406 * input_buf[(row) * w + (col)];
|
||||
|
||||
output_buf[i*output->w + j] = sum / 10000;
|
||||
}
|
||||
}
|
||||
output_buf[i * output->w + j] = sum / 10000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -322,21 +332,21 @@ void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t b
|
||||
* followed by `pyr_level` of pyramid.
|
||||
* @param[in] pyr_level - number of pyramids to be built. If 0, original image is padded and outputed.
|
||||
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
*/
|
||||
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint8_t border_size)
|
||||
{
|
||||
// Pad input image and save it as '0' pyramid level
|
||||
image_add_border(input, &output_array[0], border_size);
|
||||
// Pad input image and save it as '0' pyramid level
|
||||
image_add_border(input, &output_array[0], border_size);
|
||||
|
||||
// Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
|
||||
struct image_t temp;
|
||||
// Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
|
||||
struct image_t temp;
|
||||
|
||||
for (uint8_t i = 1; i != pyr_level + 1; i++){
|
||||
pyramid_next_level(&output_array[i-1], &temp, border_size);
|
||||
image_add_border(&temp, &output_array[i], border_size);
|
||||
image_free(&temp);
|
||||
}
|
||||
for (uint8_t i = 1; i != pyr_level + 1; i++) {
|
||||
pyramid_next_level(&output_array[i - 1], &temp, border_size);
|
||||
image_add_border(&temp, &output_array[i], border_size);
|
||||
image_free(&temp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -348,9 +358,10 @@ void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t
|
||||
* @param[in] *center Center point in subpixel coordinates
|
||||
* @param[in] subpixel_factor The subpixel factor per pixel
|
||||
* @param[in] border_size - amount of padding around image. Padding is made by reflecting image elements at the edge
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
* Example: f e d c b a | a b c d e f | f e d c b a
|
||||
*/
|
||||
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size)
|
||||
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center,
|
||||
uint32_t subpixel_factor, uint8_t border_size)
|
||||
{
|
||||
uint8_t *input_buf = (uint8_t *)input->buf;
|
||||
uint8_t *output_buf = (uint8_t *)output->buf;
|
||||
@@ -418,7 +429,8 @@ void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *
|
||||
for (uint16_t x = 1; x < input->w - 1; x++) {
|
||||
for (uint16_t y = 1; y < input->h - 1; y++) {
|
||||
dx_buf[(y - 1)*dx->w + (x - 1)] = (int16_t)input_buf[y * input->w + x + 1] - (int16_t)input_buf[y * input->w + x - 1];
|
||||
dy_buf[(y - 1)*dy->w + (x - 1)] = (int16_t)input_buf[(y + 1) * input->w + x] - (int16_t)input_buf[(y - 1) * input->w + x];
|
||||
dy_buf[(y - 1)*dy->w + (x - 1)] = (int16_t)input_buf[(y + 1) * input->w + x] - (int16_t)
|
||||
input_buf[(y - 1) * input->w + x];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -617,7 +629,8 @@ void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *
|
||||
else { distance = delta_y * 20; }
|
||||
|
||||
/* draw the line */
|
||||
for (uint16_t t = 0; /* starty >= 0 && */ starty < img->h && /* startx >= 0 && */ startx < img->w && t <= distance + 1; t++) {
|
||||
for (uint16_t t = 0; /* starty >= 0 && */ starty < img->h && /* startx >= 0 && */ startx < img->w
|
||||
&& t <= distance + 1; t++) {
|
||||
img_buf[img->w * pixel_width * starty + startx * pixel_width] = (t <= 3) ? 0 : 255;
|
||||
|
||||
if (img->type == IMAGE_YUV422) {
|
||||
|
||||
@@ -70,9 +70,11 @@ void image_free(struct image_t *img);
|
||||
void image_copy(struct image_t *input, struct image_t *output);
|
||||
void image_switch(struct image_t *a, struct image_t *b);
|
||||
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);
|
||||
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);
|
||||
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size);
|
||||
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center,
|
||||
uint32_t subpixel_factor, uint8_t border_size);
|
||||
void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy);
|
||||
void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g);
|
||||
uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff);
|
||||
|
||||
@@ -60,177 +60,176 @@
|
||||
* @param[in] pyramid_level Level of pyramid used in computation (0 == no pyramids used)
|
||||
* @return The vectors from the original *points in subpixels
|
||||
*/
|
||||
struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size,
|
||||
uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_lvl) {
|
||||
struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points,
|
||||
uint16_t *points_cnt, uint16_t half_window_size,
|
||||
uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_lvl)
|
||||
{
|
||||
|
||||
|
||||
// Pyramidal implementation of Lucas-Kanade feature tracker.
|
||||
// Uses input images to build pyramid of padded images.
|
||||
// For every pyramid level:
|
||||
// For all points:
|
||||
// (1) determine the subpixel neighborhood in the old image
|
||||
// (2) get the x- and y- gradients
|
||||
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
|
||||
// (4) iterate over taking steps in the image to minimize the error:
|
||||
// [a] get the subpixel neighborhood in the new image
|
||||
// [b] determine the image difference between the two neighborhoods
|
||||
// [c] calculate the 'b'-vector
|
||||
// [d] calculate the additional flow step and possibly terminate the iteration
|
||||
// (5) use calculated flow as initial flow estimation for next level of pyramid
|
||||
// Pyramidal implementation of Lucas-Kanade feature tracker.
|
||||
// Uses input images to build pyramid of padded images.
|
||||
// For every pyramid level:
|
||||
// For all points:
|
||||
// (1) determine the subpixel neighborhood in the old image
|
||||
// (2) get the x- and y- gradients
|
||||
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
|
||||
// (4) iterate over taking steps in the image to minimize the error:
|
||||
// [a] get the subpixel neighborhood in the new image
|
||||
// [b] determine the image difference between the two neighborhoods
|
||||
// [c] calculate the 'b'-vector
|
||||
// [d] calculate the additional flow step and possibly terminate the iteration
|
||||
// (5) use calculated flow as initial flow estimation for next level of pyramid
|
||||
|
||||
// Allocate some memory for returning the vectors
|
||||
struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
|
||||
// Allocate some memory for returning the vectors
|
||||
struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points);
|
||||
|
||||
// Determine patch sizes and initialize neighborhoods
|
||||
uint16_t patch_size = 2 * half_window_size + 1;
|
||||
uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
|
||||
uint16_t padded_patch_size = patch_size + 2;
|
||||
uint8_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images
|
||||
step_threshold = step_threshold*(subpixel_factor/100);
|
||||
// Determine patch sizes and initialize neighborhoods
|
||||
uint16_t patch_size = 2 * half_window_size + 1;
|
||||
uint32_t error_threshold = (25 * 25) * (patch_size * patch_size);
|
||||
uint16_t padded_patch_size = patch_size + 2;
|
||||
uint8_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images
|
||||
step_threshold = step_threshold * (subpixel_factor / 100);
|
||||
|
||||
// Allocate memory for image pyramids
|
||||
struct image_t *pyramid_old = malloc(sizeof(struct image_t) * (pyramid_lvl + 1));
|
||||
struct image_t *pyramid_new = malloc(sizeof(struct image_t) * (pyramid_lvl + 1));
|
||||
// Allocate memory for image pyramids
|
||||
struct image_t *pyramid_old = malloc(sizeof(struct image_t) * (pyramid_lvl + 1));
|
||||
struct image_t *pyramid_new = malloc(sizeof(struct image_t) * (pyramid_lvl + 1));
|
||||
|
||||
// Build pyramid levels
|
||||
pyramid_build(old_img, pyramid_old, pyramid_lvl, border_size);
|
||||
pyramid_build(new_img, pyramid_new, pyramid_lvl, border_size);
|
||||
// Build pyramid levels
|
||||
pyramid_build(old_img, pyramid_old, pyramid_lvl, border_size);
|
||||
pyramid_build(new_img, pyramid_new, pyramid_lvl, border_size);
|
||||
|
||||
// Create the window images
|
||||
struct image_t window_I, window_J, window_DX, window_DY, window_diff;
|
||||
image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
|
||||
image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
|
||||
image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
// Create the window images
|
||||
struct image_t window_I, window_J, window_DX, window_DY, window_diff;
|
||||
image_create(&window_I, padded_patch_size, padded_patch_size, IMAGE_GRAYSCALE);
|
||||
image_create(&window_J, patch_size, patch_size, IMAGE_GRAYSCALE);
|
||||
image_create(&window_DX, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
image_create(&window_DY, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT);
|
||||
|
||||
// Iterate through pyramid levels
|
||||
for (int8_t LVL = pyramid_lvl; LVL != -1; LVL--) {
|
||||
uint16_t points_orig = *points_cnt;
|
||||
*points_cnt = 0;
|
||||
uint16_t new_p = 0;
|
||||
// Iterate through pyramid levels
|
||||
for (int8_t LVL = pyramid_lvl; LVL != -1; LVL--) {
|
||||
uint16_t points_orig = *points_cnt;
|
||||
*points_cnt = 0;
|
||||
uint16_t new_p = 0;
|
||||
|
||||
// Calculate the amount of points to skip
|
||||
float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1;
|
||||
// Calculate the amount of points to skip
|
||||
float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1;
|
||||
|
||||
// Go through all points
|
||||
for (uint16_t i = 0; i < max_points && i < points_orig; i++)
|
||||
{
|
||||
uint16_t p = i * skip_points;
|
||||
// Go through all points
|
||||
for (uint16_t i = 0; i < max_points && i < points_orig; i++) {
|
||||
uint16_t p = i * skip_points;
|
||||
|
||||
if (LVL == pyramid_lvl)
|
||||
{
|
||||
// Convert point position on original image to a subpixel coordinate on the top pyramid level
|
||||
vectors[new_p].pos.x = (points[p].x * subpixel_factor) >> pyramid_lvl;
|
||||
vectors[new_p].pos.y = (points[p].y * subpixel_factor) >> pyramid_lvl;
|
||||
vectors[new_p].flow_x = 0;
|
||||
vectors[new_p].flow_y = 0;
|
||||
if (LVL == pyramid_lvl) {
|
||||
// Convert point position on original image to a subpixel coordinate on the top pyramid level
|
||||
vectors[new_p].pos.x = (points[p].x * subpixel_factor) >> pyramid_lvl;
|
||||
vectors[new_p].pos.y = (points[p].y * subpixel_factor) >> pyramid_lvl;
|
||||
vectors[new_p].flow_x = 0;
|
||||
vectors[new_p].flow_y = 0;
|
||||
|
||||
} else {
|
||||
// (5) use calculated flow as initial flow estimation for next level of pyramid
|
||||
vectors[new_p].pos.x = vectors[p].pos.x << 1;
|
||||
vectors[new_p].pos.y = vectors[p].pos.y << 1;
|
||||
vectors[new_p].flow_x = vectors[p].flow_x << 1;
|
||||
vectors[new_p].flow_y = vectors[p].flow_y << 1;
|
||||
}
|
||||
} else {
|
||||
// (5) use calculated flow as initial flow estimation for next level of pyramid
|
||||
vectors[new_p].pos.x = vectors[p].pos.x << 1;
|
||||
vectors[new_p].pos.y = vectors[p].pos.y << 1;
|
||||
vectors[new_p].flow_x = vectors[p].flow_x << 1;
|
||||
vectors[new_p].flow_y = vectors[p].flow_y << 1;
|
||||
}
|
||||
|
||||
// If the pixel is outside original image, do not track it
|
||||
if ((((int32_t) vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|
||||
|| ((vectors[new_p].pos.x + vectors[new_p].flow_x) > ((pyramid_new[LVL].w - 1 - 2 * border_size)* subpixel_factor))
|
||||
|| (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|
||||
|| ((vectors[new_p].pos.y + vectors[new_p].flow_y) > ((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor)))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
// If the pixel is outside original image, do not track it
|
||||
if ((((int32_t) vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|
||||
|| ((vectors[new_p].pos.x + vectors[new_p].flow_x) > ((pyramid_new[LVL].w - 1 - 2 * border_size)* subpixel_factor))
|
||||
|| (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|
||||
|| ((vectors[new_p].pos.y + vectors[new_p].flow_y) > ((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor))) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
// (1) determine the subpixel neighborhood in the old image
|
||||
image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
|
||||
// (1) determine the subpixel neighborhood in the old image
|
||||
image_subpixel_window(&pyramid_old[LVL], &window_I, &vectors[new_p].pos, subpixel_factor, border_size);
|
||||
|
||||
// (2) get the x- and y- gradients
|
||||
image_gradients(&window_I, &window_DX, &window_DY);
|
||||
// (2) get the x- and y- gradients
|
||||
image_gradients(&window_I, &window_DX, &window_DY);
|
||||
|
||||
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
|
||||
int32_t G[4];
|
||||
image_calculate_g(&window_DX, &window_DY, G);
|
||||
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
|
||||
int32_t G[4];
|
||||
image_calculate_g(&window_DX, &window_DY, G);
|
||||
|
||||
// calculate G's determinant in subpixel units:
|
||||
int32_t Det = ( G[0] * G[3] - G[1] * G[2]);
|
||||
// calculate G's determinant in subpixel units:
|
||||
int32_t Det = (G[0] * G[3] - G[1] * G[2]);
|
||||
|
||||
// Check if the determinant is bigger than 1
|
||||
if (Det < 1) {
|
||||
continue;
|
||||
}
|
||||
// Check if the determinant is bigger than 1
|
||||
if (Det < 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// (4) iterate over taking steps in the image to minimize the error:
|
||||
bool_t tracked = TRUE;
|
||||
// (4) iterate over taking steps in the image to minimize the error:
|
||||
bool_t tracked = TRUE;
|
||||
|
||||
for (uint8_t it = max_iterations; it--; ) {
|
||||
struct point_t new_point = { vectors[new_p].pos.x + vectors[new_p].flow_x,
|
||||
vectors[new_p].pos.y + vectors[new_p].flow_y };
|
||||
for (uint8_t it = max_iterations; it--;) {
|
||||
struct point_t new_point = { vectors[new_p].pos.x + vectors[new_p].flow_x,
|
||||
vectors[new_p].pos.y + vectors[new_p].flow_y
|
||||
};
|
||||
|
||||
|
||||
// If the pixel is outside original image, do not track it
|
||||
if ( (( (int32_t)vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|
||||
|| ( new_point.x > ((pyramid_new[LVL].w - 1 - 2*border_size)*subpixel_factor))
|
||||
|| (((int32_t)vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|
||||
|| ( new_point.y > ((pyramid_new[LVL].h - 1 - 2*border_size)*subpixel_factor)) )
|
||||
{
|
||||
tracked = FALSE;
|
||||
break;
|
||||
}
|
||||
// If the pixel is outside original image, do not track it
|
||||
if ((((int32_t)vectors[new_p].pos.x + vectors[new_p].flow_x) < 0)
|
||||
|| (new_point.x > ((pyramid_new[LVL].w - 1 - 2 * border_size)*subpixel_factor))
|
||||
|| (((int32_t)vectors[new_p].pos.y + vectors[new_p].flow_y) < 0)
|
||||
|| (new_point.y > ((pyramid_new[LVL].h - 1 - 2 * border_size)*subpixel_factor))) {
|
||||
tracked = FALSE;
|
||||
break;
|
||||
}
|
||||
|
||||
// [a] get the subpixel neighborhood in the new image
|
||||
image_subpixel_window(&pyramid_new[LVL], &window_J, &new_point, subpixel_factor, border_size);
|
||||
// [a] get the subpixel neighborhood in the new image
|
||||
image_subpixel_window(&pyramid_new[LVL], &window_J, &new_point, subpixel_factor, border_size);
|
||||
|
||||
// [b] determine the image difference between the two neighborhoods
|
||||
uint32_t error = image_difference(&window_I, &window_J, &window_diff);
|
||||
// [b] determine the image difference between the two neighborhoods
|
||||
uint32_t error = image_difference(&window_I, &window_J, &window_diff);
|
||||
|
||||
if (error > error_threshold && it < max_iterations / 2) {
|
||||
tracked = FALSE;
|
||||
break;
|
||||
}
|
||||
if (error > error_threshold && it < max_iterations / 2) {
|
||||
tracked = FALSE;
|
||||
break;
|
||||
}
|
||||
|
||||
int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
|
||||
int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
|
||||
int32_t b_x = image_multiply(&window_diff, &window_DX, NULL) / 255;
|
||||
int32_t b_y = image_multiply(&window_diff, &window_DY, NULL) / 255;
|
||||
|
||||
|
||||
// [d] calculate the additional flow step and possibly terminate the iteration
|
||||
int16_t step_x = (( (int64_t) G[3] * b_x - G[1] * b_y) * subpixel_factor) / Det;
|
||||
int16_t step_y = (( (int64_t) G[0] * b_y - G[2] * b_x) * subpixel_factor) / Det;
|
||||
// [d] calculate the additional flow step and possibly terminate the iteration
|
||||
int16_t step_x = (((int64_t) G[3] * b_x - G[1] * b_y) * subpixel_factor) / Det;
|
||||
int16_t step_y = (((int64_t) G[0] * b_y - G[2] * b_x) * subpixel_factor) / Det;
|
||||
|
||||
vectors[new_p].flow_x = vectors[new_p].flow_x + step_x;
|
||||
vectors[new_p].flow_y = vectors[new_p].flow_y + step_y;
|
||||
vectors[new_p].flow_x = vectors[new_p].flow_x + step_x;
|
||||
vectors[new_p].flow_y = vectors[new_p].flow_y + step_y;
|
||||
|
||||
// Check if we exceeded the treshold CHANGED made this better for 0.03
|
||||
if ((abs(step_x) + abs(step_y)) < step_threshold) {
|
||||
break;
|
||||
}
|
||||
} // lucas kanade step iteration
|
||||
// Check if we exceeded the treshold CHANGED made this better for 0.03
|
||||
if ((abs(step_x) + abs(step_y)) < step_threshold) {
|
||||
break;
|
||||
}
|
||||
} // lucas kanade step iteration
|
||||
|
||||
// If we tracked the point we update the index and the count
|
||||
if (tracked) {
|
||||
new_p++;
|
||||
(*points_cnt)++;
|
||||
}
|
||||
} // go through all points
|
||||
// If we tracked the point we update the index and the count
|
||||
if (tracked) {
|
||||
new_p++;
|
||||
(*points_cnt)++;
|
||||
}
|
||||
} // go through all points
|
||||
|
||||
} // LVL of pyramid
|
||||
} // LVL of pyramid
|
||||
|
||||
// Free the images
|
||||
image_free(&window_I);
|
||||
image_free(&window_J);
|
||||
image_free(&window_DX);
|
||||
image_free(&window_DY);
|
||||
image_free(&window_diff);
|
||||
// Free the images
|
||||
image_free(&window_I);
|
||||
image_free(&window_J);
|
||||
image_free(&window_DX);
|
||||
image_free(&window_DY);
|
||||
image_free(&window_diff);
|
||||
|
||||
for (int8_t i = pyramid_lvl; i!= -1; i--){
|
||||
image_free(&pyramid_old[i]);
|
||||
image_free(&pyramid_new[i]);
|
||||
}
|
||||
pyramid_old = NULL;
|
||||
pyramid_new = NULL;
|
||||
for (int8_t i = pyramid_lvl; i != -1; i--) {
|
||||
image_free(&pyramid_old[i]);
|
||||
image_free(&pyramid_new[i]);
|
||||
}
|
||||
pyramid_old = NULL;
|
||||
pyramid_new = NULL;
|
||||
|
||||
// Return the vectors
|
||||
return vectors;
|
||||
// Return the vectors
|
||||
return vectors;
|
||||
}
|
||||
|
||||
@@ -34,7 +34,8 @@
|
||||
#include "std.h"
|
||||
#include "image.h"
|
||||
|
||||
struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size,
|
||||
uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_lvl);
|
||||
struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points,
|
||||
uint16_t *points_cnt, uint16_t half_window_size,
|
||||
uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_lvl);
|
||||
|
||||
#endif /* OPTIC_FLOW_INT_H */
|
||||
|
||||
@@ -51,7 +51,7 @@ struct opticflow_t {
|
||||
uint16_t subpixel_factor; ///< The amount of subpixels per pixel
|
||||
uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do
|
||||
uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop
|
||||
uint8_t pyramid_lvl; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)
|
||||
uint8_t pyramid_lvl; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)
|
||||
|
||||
uint8_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track
|
||||
bool_t fast9_adaptive; ///< Whether the FAST9 threshold should be adaptive
|
||||
@@ -62,7 +62,8 @@ struct opticflow_t {
|
||||
|
||||
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h);
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *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);
|
||||
|
||||
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
struct opticflow_result_t *result);
|
||||
|
||||
Reference in New Issue
Block a user